From c738981778fa9415608e5a7c4b6aac9def6ca3ef Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Fri, 29 Jan 2021 15:15:41 -0300 Subject: [PATCH] World exporter (#474) * Added world exporter Signed-off-by: Gonzalo de Pedro * Fixed CMake Signed-off-by: Gonzalo de Pedro * Changes based on review Signed-off-by: Gonzalo de Pedro * Added example world Signed-off-by: Nate Koenig * bump required ign-common version Signed-off-by: Louise Poubel * PR updates Signed-off-by: Nate Koenig * Update documentation to have simulation self terminate Signed-off-by: Nate Koenig * Rename world_export to collada_world_exporter Signed-off-by: Nate Koenig * Finish world exporter renaming Signed-off-by: Nate Koenig * Revert change Signed-off-by: Nate Koenig * Added a tutorial Signed-off-by: Nate Koenig * Update transform Signed-off-by: Nate Koenig * Added message Signed-off-by: Nate Koenig * Added a test Signed-off-by: Nate Koenig * Added more shapes Signed-off-by: Nate Koenig * Cleanup in two locations Signed-off-by: Nate Koenig * Fix build Signed-off-by: Nate Koenig * Apply scale, and fix codecheck Signed-off-by: Nate Koenig Co-authored-by: Louise Poubel Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- examples/worlds/collada_world_exporter.sdf | 180 ++++++++++++ src/systems/CMakeLists.txt | 1 + .../collada_world_exporter/CMakeLists.txt | 4 + .../ColladaWorldExporter.cc | 266 ++++++++++++++++++ .../ColladaWorldExporter.hh | 59 ++++ test/integration/CMakeLists.txt | 1 + test/integration/collada_world_exporter.cc | 83 ++++++ test/integration/diff_drive_system.cc | 4 +- test/worlds/collada_world_exporter.sdf | 150 ++++++++++ tutorials.md.in | 2 + tutorials/collada_world_exporter.md | 25 ++ 12 files changed, 775 insertions(+), 2 deletions(-) create mode 100644 examples/worlds/collada_world_exporter.sdf create mode 100644 src/systems/collada_world_exporter/CMakeLists.txt create mode 100644 src/systems/collada_world_exporter/ColladaWorldExporter.cc create mode 100644 src/systems/collada_world_exporter/ColladaWorldExporter.hh create mode 100644 test/integration/collada_world_exporter.cc create mode 100644 test/worlds/collada_world_exporter.sdf create mode 100644 tutorials/collada_world_exporter.md diff --git a/CMakeLists.txt b/CMakeLists.txt index 028af75f64..c3dddc08d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ set(IGN_MSGS_VER ${ignition-msgs5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-common # Always use the profiler component to get the headers, regardless of status. -ign_find_package(ignition-common3 VERSION 3.4 +ign_find_package(ignition-common3 VERSION 3.8 COMPONENTS profiler events diff --git a/examples/worlds/collada_world_exporter.sdf b/examples/worlds/collada_world_exporter.sdf new file mode 100644 index 0000000000..cd4b089ddd --- /dev/null +++ b/examples/worlds/collada_world_exporter.sdf @@ -0,0 +1,180 @@ + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index d63a8bf40d..1d6097c069 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -76,6 +76,7 @@ add_subdirectory(apply_joint_force) add_subdirectory(battery_plugin) add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) +add_subdirectory(collada_world_exporter) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) diff --git a/src/systems/collada_world_exporter/CMakeLists.txt b/src/systems/collada_world_exporter/CMakeLists.txt new file mode 100644 index 0000000000..a4bae60924 --- /dev/null +++ b/src/systems/collada_world_exporter/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(collada-world-exporter + SOURCES + ColladaWorldExporter.cc +) diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.cc b/src/systems/collada_world_exporter/ColladaWorldExporter.cc new file mode 100644 index 0000000000..0ab619f899 --- /dev/null +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.cc @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include "ColladaWorldExporter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + + +class ignition::gazebo::systems::ColladaWorldExporterPrivate +{ + // Default constructor + public: ColladaWorldExporterPrivate() = default; + + /// \brief Has the world already been exported?. + private: bool exported{false}; + + /// \brief Exports the world to a mesh. + /// \param[_ecm] _ecm Mutable reference to the EntityComponentManager. + public: void Export(const EntityComponentManager &_ecm) + { + if (this->exported) return; + + common::Mesh worldMesh; + std::vector subMeshMatrix; + + _ecm.Each( + [&](const Entity /*& _entity*/, + const components::World *, + const components::Name * _name)->bool + { + worldMesh.SetName(_name->Data()); + return true; + }); + + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Geometry *_geom, + const components::Transparency *_transparency)->bool + { + std::string name = _name->Data().empty() ? std::to_string(_entity) : + _name->Data(); + + math::Pose3d worldPose = gazebo::worldPose(_entity, _ecm); + + common::MaterialPtr mat = std::make_shared(); + auto material = _ecm.Component(_entity); + if (material != nullptr) + { + mat->SetDiffuse(material->Data().Diffuse()); + mat->SetAmbient(material->Data().Ambient()); + mat->SetEmissive(material->Data().Emissive()); + mat->SetSpecular(material->Data().Specular()); + } + mat->SetTransparency(_transparency->Data()); + + const ignition::common::Mesh *mesh; + std::weak_ptr subm; + math::Vector3d scale; + math::Matrix4d matrix(worldPose); + ignition::common::MeshManager *meshManager = + ignition::common::MeshManager::Instance(); + + auto addSubmeshFunc = [&](int i) { + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); + subm.lock()->SetMaterialIndex(i); + subm.lock()->Scale(scale); + subMeshMatrix.push_back(matrix); + }; + + if (_geom->Data().Type() == sdf::GeometryType::BOX) + { + if (meshManager->HasMesh("unit_box")) + { + mesh = meshManager->MeshByName("unit_box"); + scale = _geom->Data().BoxShape()->Size(); + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::CYLINDER) + { + if (meshManager->HasMesh("unit_cylinder")) + { + mesh = meshManager->MeshByName("unit_cylinder"); + scale.X() = _geom->Data().CylinderShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _geom->Data().CylinderShape()->Length(); + + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::PLANE) + { + if (meshManager->HasMesh("unit_plane")) + { + // Create a rotation for the plane mesh to account + // for the normal vector. + mesh = meshManager->MeshByName("unit_plane"); + + scale.X() = _geom->Data().PlaneShape()->Size().X(); + scale.Y() = _geom->Data().PlaneShape()->Size().Y(); + + // // The rotation is the angle between the +z(0,0,1) vector and the + // // normal, which are both expressed in the local (Visual) frame. + math::Vector3d normal = _geom->Data().PlaneShape()->Normal(); + math::Quaterniond normalRot; + normalRot.From2Axes(math::Vector3d::UnitZ, normal.Normalized()); + worldPose.Rot() = worldPose.Rot() * normalRot; + + matrix = math::Matrix4d(worldPose); + + int i = worldMesh.AddMaterial(mat); + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::SPHERE) + { + if (meshManager->HasMesh("unit_sphere")) + { + mesh = meshManager->MeshByName("unit_sphere"); + + scale.X() = _geom->Data().SphereShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = scale.X(); + + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::MESH) + { + auto fullPath = asFullPath(_geom->Data().MeshShape()->Uri(), + _geom->Data().MeshShape()->FilePath()); + + if (fullPath.empty()) + { + ignerr << "Mesh geometry missing uri" << std::endl; + return true; + } + mesh = meshManager->Load(fullPath); + + if (!mesh) { + ignerr << "mesh not found!" << std::endl; + return true; + } + + for (unsigned int k = 0; k < mesh->SubMeshCount(); k++) + { + auto subMeshLock = mesh->SubMeshByIndex(k).lock(); + int j = subMeshLock->MaterialIndex(); + + int i = 0; + if (j != -1) + { + i = worldMesh.IndexOfMaterial(mesh->MaterialByIndex(j).get()); + if (i < 0) + { + i = worldMesh.AddMaterial(mesh->MaterialByIndex(j)); + } + } + else + { + i = worldMesh.AddMaterial(mat); + } + + scale = _geom->Data().MeshShape()->Scale(); + + addSubmeshFunc(i); + } + } + else + { + ignwarn << "Unsupported geometry type" << std::endl; + } + + return true; + }); + + common::ColladaExporter exporter; + exporter.Export(&worldMesh, "./" + worldMesh.Name(), true, + subMeshMatrix); + ignmsg << "The world has been exported into the " + << "./" + worldMesh.Name() << " directory." << std::endl; + this->exported = true; + } +}; + +///////////////////////////////////////////////// +ColladaWorldExporter::ColladaWorldExporter() : System(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ColladaWorldExporter::~ColladaWorldExporter() = default; + +///////////////////////////////////////////////// +void ColladaWorldExporter::PostUpdate(const UpdateInfo & /*_info*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->Export(_ecm); +} + +IGNITION_ADD_PLUGIN(ColladaWorldExporter, + System, + ColladaWorldExporter::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ColladaWorldExporter, + "ignition::gazebo::systems::ColladaWorldExporter") diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.hh b/src/systems/collada_world_exporter/ColladaWorldExporter.hh new file mode 100644 index 0000000000..c31a175b0a --- /dev/null +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_COLLADAWORLDEXPORTER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_COLLADAWORLDEXPORTER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ColladaWorldExporterPrivate; + + /// \brief A plugin that exports a world to a mesh. + /// When loaded the plugin will dump a mesh containing all the models in + /// the world to the current directory. + class IGNITION_GAZEBO_VISIBLE ColladaWorldExporter: + public System, + public ISystemPostUpdate + { + /// \brief Constructor + public: ColladaWorldExporter(); + + /// \brief Destructor + public: ~ColladaWorldExporter() final; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index a12e65d7d3..afcd29cfb4 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -7,6 +7,7 @@ set(tests battery_plugin.cc breadcrumbs.cc buoyancy.cc + collada_world_exporter.cc components.cc contact_system.cc detachable_joint.cc diff --git a/test/integration/collada_world_exporter.cc b/test/integration/collada_world_exporter.cc new file mode 100644 index 0000000000..ae677413c0 --- /dev/null +++ b/test/integration/collada_world_exporter.cc @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "helpers/UniqueTestDirectoryEnv.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +class ColladaWorldExporterFixture : public ::testing::Test +{ + public: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } + + public: void LoadWorld(const std::string &_path) + { + ServerConfig serverConfig; + serverConfig.SetResourceCache(test::UniqueTestDirectoryEnv::Path()); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, _path)); + + std::cout << "Loading: " << serverConfig.SdfFile() << std::endl; + this->server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + } + + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +TEST_F(ColladaWorldExporterFixture, ExportWorld) +{ + this->LoadWorld(common::joinPaths("test", "worlds", + "collada_world_exporter.sdf")); + + // Cleanup + common::removeAll("./collada_world_exporter_box_test"); + + // The export directory shouldn't exist. + EXPECT_FALSE(common::exists("./collada_world_exporter_box_test")); + + // Run one iteration which should export the world. + server->Run(true, 1, false); + + // The export directory should now exist. + EXPECT_TRUE(common::exists("./collada_world_exporter_box_test")); + + // Cleanup + common::removeAll("./collada_world_exporter_box_test"); +} + +///////////////////////////////////////////////// +/// Main +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("save_world_test_cache")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 35aa854196..027648c0e8 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -383,7 +383,8 @@ TEST_P(DiffDriveTest, OdomFrameId) int sleep = 0; int maxSleep = 30; - for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -620,6 +621,7 @@ TEST_P(DiffDriveTest, Pose_VCustomTfTopic) int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/worlds/collada_world_exporter.sdf b/test/worlds/collada_world_exporter.sdf new file mode 100644 index 0000000000..b65709b628 --- /dev/null +++ b/test/worlds/collada_world_exporter.sdf @@ -0,0 +1,150 @@ + + + + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index f8c32bdf00..ab230deb0d 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -24,6 +24,8 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation. * \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. +* \subpage collada_world_exporter "Collada World Exporter": Export an entire +world to a single Collada mesh. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. **Migration from Gazebo classic** diff --git a/tutorials/collada_world_exporter.md b/tutorials/collada_world_exporter.md new file mode 100644 index 0000000000..e1bdd4e60a --- /dev/null +++ b/tutorials/collada_world_exporter.md @@ -0,0 +1,25 @@ +\page collada_world_exporter Collada World Exporter + +The Collada world exporter is a world plugin that automatically generates +a Collada mesh consisting of the models within a world SDF file. This plugin +can be useful if you'd like to share an SDF world without requiring an SDF +loader. + +## Using the Collada World Exporter + +1. Add the following lines as a child to the `` tag in an SDF file. +``` + + +``` + +2. Run the world using +``` +ign gazebo -v 4 -s -r --iterations 1 WORLD_FILE_NAME +``` + +3. A subdirectory, named after the world, has been created in the current working directory. Within this subdirectory is the mesh and materials for the world. + +Refer to the [collada_world_exporter.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/examples/worlds/collada_world_exporter.sdf) example.