diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index c8809eb33d..41df71ff32 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -24,7 +24,6 @@ libtinyxml2-dev libxi-dev libxmu-dev libpython3-dev -python3-distutils python3-gz-math8 python3-gz-msgs11 python3-gz-transport14 diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 0000000000..4bd4a9aa0b --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/CMakeLists.txt b/CMakeLists.txt index 25ad14d382..375fea97c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -225,11 +225,17 @@ else() endif() endif() # Plugin install dirs +set(GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR + ${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins +) set(GZ_SIM_PLUGIN_INSTALL_DIR - ${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins + ${CMAKE_INSTALL_PREFIX}/${GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR} +) +set(GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR + ${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui ) set(GZ_SIM_GUI_PLUGIN_INSTALL_DIR - ${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins/gui + ${CMAKE_INSTALL_PREFIX}/${GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR} ) #============================================================================ diff --git a/Changelog.md b/Changelog.md index 8e8cc35c3b..33b42afb6b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,55 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.3.0 (2024-04-11) + +1. Use relative install paths for plugin shared libraries and gz-tools data + * [Pull request #2358](https://github.com/gazebosim/gz-sim/pull/2358) + +1. Use `steer_p_gain` for UpdateVelocity steer joint speed + * [Pull request #2355](https://github.com/gazebosim/gz-sim/pull/2355) + +1. Fix TriggeredPublisher test + * [Pull request #2354](https://github.com/gazebosim/gz-sim/pull/2354) + +1. Use SetComponentData to simplify code and improve coverage + * [Pull request #2360](https://github.com/gazebosim/gz-sim/pull/2360) + +1. Remove unnecessary sleep + * [Pull request #2357](https://github.com/gazebosim/gz-sim/pull/2357) + +1. Fixed undefined behavior in thruster.cc + * [Pull request #2350](https://github.com/gazebosim/gz-sim/pull/2350) + +1. Added mutex to protect stored time variables + * [Pull request #2345](https://github.com/gazebosim/gz-sim/pull/2345) + +1. Fixed turning error in ackermann steering + * [Pull request #2342](https://github.com/gazebosim/gz-sim/pull/2342) + +1. Check null mesh + * [Pull request #2341](https://github.com/gazebosim/gz-sim/pull/2341) + +1. Publish step size in world stats topic + * [Pull request #2340](https://github.com/gazebosim/gz-sim/pull/2340) + +### Gazebo Sim 8.2.0 (2024-03-14) + +1. Add reference to joint_controller.md tutorial. + * [Pull request #2333](https://github.com/gazebosim/gz-sim/pull/2333) + +1. Fix wget in maritime tutorials + * [Pull request #2330](https://github.com/gazebosim/gz-sim/pull/2330) + +1. Add entity and sdf parameters to Server's AddSystem interface + * [Pull request #2324](https://github.com/gazebosim/gz-sim/pull/2324) + +1. Add entity validation to OdometryPublisher + * [Pull request #2326](https://github.com/gazebosim/gz-sim/pull/2326) + +1. Fix typo in Joint.hh + * [Pull request #2310](https://github.com/gazebosim/gz-sim/pull/2310) + ### Gazebo Sim 8.1.0 (2024-02-06) 1. Add tutorial for using components in systems diff --git a/Migration.md b/Migration.md index a1df392c5c..2e55298ea9 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,14 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Gazebo Sim 8.x to 9.0 + + * **Modified**: + + In the Physics system, all `*VelocityCmd` components are now deleted after + each time step, whereas previously the component values were set to `0` + after each time step. Persistent velocity commands should be reapplied at + each time step. + ## Gazebo Sim 7.x to 8.0 * **Deprecated** + `gz::sim::components::Factory::Register(const std::string &_type, ComponentDescriptorBase *_compDesc)` and diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 6682fed462..d7e8c722a0 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -27,10 +27,11 @@ add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_cl add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) target_link_libraries(${PROJECT_NAME} - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} - PRIVATE odometer + PRIVATE + gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} + odometer ) target_include_directories(${PROJECT_NAME} PUBLIC ${sensors_clone_SOURCE_DIR}/examples/custom_sensor) diff --git a/examples/standalone/gtest_setup/CMakeLists.txt b/examples/standalone/gtest_setup/CMakeLists.txt index 444b069b09..eb08a179a2 100644 --- a/examples/standalone/gtest_setup/CMakeLists.txt +++ b/examples/standalone/gtest_setup/CMakeLists.txt @@ -10,13 +10,14 @@ set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR}) include(FetchContent) FetchContent_Declare( googletest - URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + # Version 1.14. Use commit hash to prevent tag relocation + URL https://github.com/google/googletest/archive/f8d7d77c06936315286eb55f8de22cd23c188571.zip ) set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) enable_testing() -include(Dart) # Generate tests foreach(TEST_TARGET diff --git a/examples/worlds/dvl_world.sdf b/examples/worlds/dvl_world.sdf new file mode 100644 index 0000000000..0b599a3cb4 --- /dev/null +++ b/examples/worlds/dvl_world.sdf @@ -0,0 +1,291 @@ + + + + + + + 0.0 1.0 1.0 + + 0.0 0.7 0.8 + + false + + + + 0.001 + 1.0 + + + + + + + + + 1000 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + true + + + + + 0 0 1 + + 300000 300000 + + + 1.0 + + + + + + true + 0 0 -100 0 0 0 + + + + + 0 0 1 + + 300000 300000 + + + + + + 0.5 0.5 0.5 + 0.5 0.5 0.5 + + + + 0 0 1 + + 300000 300000 + + + + + + + + 0 0 -80 0 0 1.57 + https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + -0.60 0 -0.16 0 0 180 + 1 + 1 + /dvl/velocity + + phased_array + + + 2 + 45 + 30 + + + 2 + 135 + 30 + + + 2 + -45 + 30 + + + 2 + -135 + 30 + + + + + best + + + 0.002 + + false + + + + 0.01 + 100. + 0.1 + + 0 0 0 0 0 -1.570796 + + + + + + + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + 0 + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + diff --git a/examples/worlds/lighter_than_air_blimp.sdf b/examples/worlds/lighter_than_air_blimp.sdf new file mode 100644 index 0000000000..b3558ddff2 --- /dev/null +++ b/examples/worlds/lighter_than_air_blimp.sdf @@ -0,0 +1,78 @@ + + + + + + 0.001 + + 1 + + + 0 0 -9.81 + + + + + + + + + + 1.097 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 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 + + + + + + + https://fuel.gazebosim.org/1.0/hkotze/models/airship + + + + diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 842e287025..18037896a7 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -277,6 +278,14 @@ namespace gz public: std::optional WorldInertiaMatrix( const EntityComponentManager &_ecm) const; + /// \brief Get the fluid added mass matrix in the world frame. + /// \param[in] _ecm Entity-component manager. + /// \return Fluide added matrix in world frame, returns nullopt if link + /// does not have components components::Inertial and + /// components::WorldPose. + public: std::optional WorldFluidAddedMassMatrix( + const EntityComponentManager &_ecm) const; + /// \brief Get the rotational and translational kinetic energy of the /// link with respect to the world frame. /// \param[in] _ecm Entity-component manager. diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index bfb0e3e362..d32e0c4fcb 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -60,6 +60,14 @@ namespace gz kSdfString, }; + /// \brief SDF error behavior + public: enum class SdfErrorBehavior + { + /// \brief Exit the server immediately + EXIT_IMMEDIATELY, + /// \brief Continue loading the server if possible + CONTINUE_LOADING + }; class PluginInfoPrivate; /// \brief Information about a plugin that should be loaded by the @@ -386,7 +394,17 @@ namespace gz const std::string &_apiBackend); /// \return Api backend for gui. See SetRenderEngineGuiApiBackend() - const std::string &RenderEngineGuiApiBackend() const; + public: const std::string &RenderEngineGuiApiBackend() const; + + /// \brief Set the server behavior when SDF errors are encountered while + //// loading the server. + /// \param[in] _behavior Server behavior when SDF errors are encounted. + public: void SetBehaviorOnSdfErrors(SdfErrorBehavior _behavior); + + /// \brief Get the behavior when SDF errors are encountered while + //// loading the server. + /// \return Server behavior when SDF errors are encounted. + public: SdfErrorBehavior BehaviorOnSdfErrors() const; /// \brief Instruct simulation to attach a plugin to a specific /// entity when simulation starts. diff --git a/include/gz/sim/components/Factory.hh b/include/gz/sim/components/Factory.hh index 393c770a9d..5f27a130bd 100644 --- a/include/gz/sim/components/Factory.hh +++ b/include/gz/sim/components/Factory.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_COMPONENTS_FACTORY_HH_ #define GZ_SIM_COMPONENTS_FACTORY_HH_ +#include #include #include #include diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000..913279ea55 --- /dev/null +++ b/package.xml @@ -0,0 +1,55 @@ + + + + gz-sim9 + 9.0.0 + Gazebo Sim : A Robotic Simulator + Michael Carroll + Apache License 2.0 + https://github.com/gazebosim/gz-sim + + cmake + + benchmark + glut + gz-cmake4 + gz-common6 + gz-fuel_tools10 + gz-gui9 + gz-math8 + gz-msgs11 + gz-physics8 + gz-plugin3 + gz-rendering9 + gz-sensors9 + gz-tools2 + gz-transport14 + gz-utils3 + libfreeimage-dev + libglew-dev + libxi-dev + libxmu-dev + protobuf-dev + pybind11-dev + python3-distutils + qml-module-qt-labs-folderlistmodel + qml-module-qt-labs-settings + qml-module-qtgraphicaleffects + qml-module-qtquick-controls2 + qml-module-qtquick-controls + qml-module-qtquick-dialogs + qml-module-qtquick-layouts + qml-module-qtquick2 + qtbase5-dev + qtdeclarative5-dev + sdformat15 + tinyxml2 + uuid + + xvfb + python3-pytest + + + cmake + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 879a01bffe..7f0aaf946a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -124,7 +124,6 @@ set (gtest_sources SimulationRunner_TEST.cc SystemLoader_TEST.cc SystemManager_TEST.cc - System_TEST.cc TestFixture_TEST.cc Util_TEST.cc World_TEST.cc @@ -245,10 +244,6 @@ target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} $ ) -set(GZ_SIM_PLUGIN_INSTALL_DIR - ${CMAKE_INSTALL_PREFIX}/${GZ_LIB_INSTALL_DIR}/gz-${GZ_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins -) - include_directories(${PROJECT_SOURCE_DIR}/test) # Build the unit tests diff --git a/src/Link.cc b/src/Link.cc index 4fef211baf..348923a44a 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -356,6 +356,18 @@ std::optional Link::WorldInertiaMatrix( math::Inertiald(inertial->Data().MassMatrix(), comWorldPose).Moi()); } +std::optional Link::WorldFluidAddedMassMatrix( + const EntityComponentManager &_ecm) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.Component(this->dataPtr->id); + + if (!worldPose || !inertial) + return std::nullopt; + + return inertial->Data().FluidAddedMass(); +} + ////////////////////////////////////////////////// std::optional Link::WorldKineticEnergy( const EntityComponentManager &_ecm) const diff --git a/src/MeshInertiaCalculator.cc b/src/MeshInertiaCalculator.cc index 23d5e95310..47b7c3c230 100644 --- a/src/MeshInertiaCalculator.cc +++ b/src/MeshInertiaCalculator.cc @@ -218,6 +218,11 @@ std::optional MeshInertiaCalculator::operator() // Load the Mesh gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); mesh = meshManager->Load(fullPath); + if (!mesh) + { + gzerr << "Failed to load mesh: " << fullPath << std::endl; + return std::nullopt; + } std::vector meshTriangles; gz::math::MassMatrix3d meshMassMatrix; gz::math::Pose3d centreOfMass; diff --git a/src/Server.cc b/src/Server.cc index 7aca6f33f3..e2286945e7 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -121,6 +121,7 @@ Server::Server(const ServerConfig &_config) } gzmsg << msg; sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); + sdfParserConfig.SetStoreResolvedURIs(true); sdfParserConfig.SetCalculateInertialConfiguration( sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); errors = this->dataPtr->sdfRoot.LoadSdfString( @@ -145,6 +146,7 @@ Server::Server(const ServerConfig &_config) sdf::Root sdfRoot; sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); + sdfParserConfig.SetStoreResolvedURIs(true); sdfParserConfig.SetCalculateInertialConfiguration( sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); @@ -156,7 +158,8 @@ Server::Server(const ServerConfig &_config) // a black screen (search for "Async resource download" in // 'src/gui_main.cc'. errors = sdfRoot.Load(filePath, sdfParserConfig); - if (errors.empty()) { + if (errors.empty() || _config.BehaviorOnSdfErrors() != + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) { if (sdfRoot.Model() == nullptr) { this->dataPtr->sdfRoot = std::move(sdfRoot); } @@ -171,7 +174,9 @@ Server::Server(const ServerConfig &_config) return; } world->AddModel(*sdfRoot.Model()); - if (errors.empty()) { + if (errors.empty() || _config.BehaviorOnSdfErrors() != + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) + { errors = this->dataPtr->sdfRoot.UpdateGraphs(); } } @@ -196,7 +201,11 @@ Server::Server(const ServerConfig &_config) { for (auto &err : errors) gzerr << err << "\n"; - return; + if (_config.BehaviorOnSdfErrors() == + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) + { + return; + } } // Add record plugin diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index b8a33fd965..ae587f2703 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -208,7 +208,8 @@ class gz::sim::ServerConfigPrivate seed(_cfg->seed), logRecordTopics(_cfg->logRecordTopics), isHeadlessRendering(_cfg->isHeadlessRendering), - source(_cfg->source){ } + source(_cfg->source), + behaviorOnSdfErrors(_cfg->behaviorOnSdfErrors){ } // \brief The SDF file that the server should load public: std::string sdfFile = ""; @@ -292,6 +293,10 @@ class gz::sim::ServerConfigPrivate /// \brief Type of source used. public: ServerConfig::SourceType source{ServerConfig::SourceType::kNone}; + + /// \brief Server loading behavior in presence of SDF errors. + public: ServerConfig::SdfErrorBehavior behaviorOnSdfErrors{ + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY}; }; ////////////////////////////////////////////////// @@ -598,6 +603,19 @@ const std::string &ServerConfig::RenderEngineGuiApiBackend() const return this->dataPtr->renderEngineGuiApiBackend; } +////////////////////////////////////////////////// +void ServerConfig::SetBehaviorOnSdfErrors( + ServerConfig::SdfErrorBehavior _behavior) +{ + this->dataPtr->behaviorOnSdfErrors = _behavior; +} + +////////////////////////////////////////////////// +ServerConfig::SdfErrorBehavior ServerConfig::BehaviorOnSdfErrors() const +{ + return this->dataPtr->behaviorOnSdfErrors; +} + ///////////////////////////////////////////////// void ServerConfig::AddPlugin(const ServerConfig::PluginInfo &_info) { diff --git a/src/Server_Rendering_TEST.cc b/src/Server_Rendering_TEST.cc index b82b0690ee..bad8c2d323 100644 --- a/src/Server_Rendering_TEST.cc +++ b/src/Server_Rendering_TEST.cc @@ -130,7 +130,10 @@ TEST_F(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(LoadSdfModelRelativeUri)) }; gz::sim::ServerConfig serverConfig; - + serverConfig.SetBehaviorOnSdfErrors( + ServerConfig::SdfErrorBehavior::CONTINUE_LOADING); + EXPECT_EQ(ServerConfig::SdfErrorBehavior::CONTINUE_LOADING, + serverConfig.BehaviorOnSdfErrors()); serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models", "relative_resource_uri", "model2.sdf")); @@ -162,4 +165,16 @@ TEST_F(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(LoadSdfModelRelativeUri)) } ASSERT_TRUE(server.RunOnce()); ASSERT_TRUE(server.RunOnce(false)); + + // Tell server to stop loading if there are SDF errors + // Server should not load because V2's visual mesh URI can not be resolved + serverConfig.SetBehaviorOnSdfErrors( + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY); + EXPECT_EQ(ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY, + serverConfig.BehaviorOnSdfErrors()); + sim::Server server2 = Server(serverConfig); + EXPECT_FALSE(server2.HasEntity("relative_resource_uri")); + EXPECT_FALSE(server2.HasEntity("L1")); + EXPECT_FALSE(server2.HasEntity("V1")); + EXPECT_FALSE(server2.HasEntity("V2")); } diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index e2fba5e054..a7c327f0db 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1012,8 +1012,11 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) if (mesh) { - EXPECT_EQ("model://scheme_resource_uri/meshes/box.dae", - mesh->Uri()); + // StoreResolvedURIs is set to true so expect full path + EXPECT_NE(std::string::npos, + mesh->Uri().find("scheme_resource_uri/meshes/box.dae")); + EXPECT_FALSE(common::isRelativePath(mesh->Uri())); + EXPECT_TRUE(common::isFile(mesh->Uri())); } eachCount++; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 4ee08be76a..a5ba51e96c 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -463,6 +463,8 @@ void SimulationRunner::PublishStats() msg.set_paused(this->currentInfo.paused); + msgs::Set(msg.mutable_step_size(), this->currentInfo.dt); + if (this->Stepping()) { // (deprecated) Remove this header in Gazebo H diff --git a/src/System_TEST.cc b/src/System_TEST.cc deleted file mode 100644 index e238456085..0000000000 --- a/src/System_TEST.cc +++ /dev/null @@ -1,27 +0,0 @@ -/* - * Copyright (C) 2018 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 "gz/sim/System.hh" - -using namespace gz; - -///////////////////////////////////////////////// -TEST(System, Constructor) -{ -} diff --git a/src/Util.cc b/src/Util.cc index e7b444bfe7..8adfedbee7 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -397,12 +397,19 @@ std::string asFullPath(const std::string &_uri, const std::string &_filePath) { return _uri; } +#elif defined(_WIN32) + if (_uri.find("://") != std::string::npos || + common::isFile(_uri)) + { + return _uri; + } #else if (_uri.find("://") != std::string::npos || !common::isRelativePath(_uri)) { return _uri; } + #endif // When SDF is loaded from a string instead of a file diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index facaaca434..021cc37dd9 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -41,7 +41,7 @@ configure_file( install( FILES ${CMAKE_CURRENT_BINARY_DIR}/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml DESTINATION - ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/) + ${CMAKE_INSTALL_DATAROOTDIR}/gz/) #=============================================================================== # Used for the installed model command version. @@ -69,7 +69,7 @@ configure_file( "model.yaml.in" ${model_configured}) -install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/) +install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/gz/) #=============================================================================== @@ -146,7 +146,7 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/sim${PROJECT_VERSION_MAJOR}.bash_completion.sh DESTINATION - ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) + ${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) configure_file( "model.bash_completion.sh" @@ -155,4 +155,4 @@ install( FILES ${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.bash_completion.sh DESTINATION - ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) + ${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${GZ_TOOLS_VER}.completion.d) diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 93cf416713..837f929c7a 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -124,7 +124,7 @@ function(gz_add_gui_plugin plugin_name) endif() endif() - install (TARGETS ${plugin_name} DESTINATION ${GZ_SIM_GUI_PLUGIN_INSTALL_DIR}) + install (TARGETS ${plugin_name} DESTINATION ${GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR}) endfunction() add_subdirectory(modules) diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index 24a607d20f..a2ca6ce79f 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include #include diff --git a/src/gui/plugins/modules/CMakeLists.txt b/src/gui/plugins/modules/CMakeLists.txt index 3874f36f46..4721a155cd 100644 --- a/src/gui/plugins/modules/CMakeLists.txt +++ b/src/gui/plugins/modules/CMakeLists.txt @@ -7,5 +7,5 @@ gz_add_gui_library(EntityContextMenu QT_HEADERS EntityContextMenu.hh ) -install (TARGETS EntityContextMenu DESTINATION ${GZ_SIM_GUI_PLUGIN_INSTALL_DIR}/${module_name}) -install (FILES qmldir DESTINATION ${GZ_SIM_GUI_PLUGIN_INSTALL_DIR}/${module_name}) +install (TARGETS EntityContextMenu DESTINATION ${GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR}/${module_name}) +install (FILES qmldir DESTINATION ${GZ_SIM_GUI_PLUGIN_RELATIVE_INSTALL_DIR}/${module_name}) diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 812a787540..6af4435972 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -19,10 +19,12 @@ #include "EntityContextMenu.hh" #include -#include +#include #include +#include #include +#include #include #include @@ -35,14 +37,21 @@ namespace gz::sim /// \brief Private data class for EntityContextMenu class EntityContextMenuPrivate { + + /// \brief Protects variable changed through services. + public: std::mutex mutex; + /// \brief Gazebo communication node. public: transport::Node node; /// \brief Move to service name public: std::string moveToService; - /// \brief Follow service name - public: std::string followService; + /// \brief Track topic name + public: std::string trackTopic; + + /// \brief Currently tracked topic name + public: std::string currentTrackTopic; /// \brief Remove service name public: std::string removeService; @@ -76,12 +85,37 @@ namespace gz::sim /// \brief Name of world. public: std::string worldName; + + /// \brief Storing last follow target for look at. + public: std::string followTargetLookAt; + + /// \brief Flag used to disable look at when not following target. + public: bool followingTarget{false}; + + /// \brief /gui/track publisher + public: transport::Node::Publisher trackPub; }; } using namespace gz; using namespace sim; +///////////////////////////////////////////////// +void EntityContextMenu::OnCurrentlyTrackedSub(const msgs::CameraTrack &_msg) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->followingTarget = false; + if (_msg.track_mode() == gz::msgs::CameraTrack::FOLLOW || + _msg.track_mode() == gz::msgs::CameraTrack::FOLLOW_LOOK_AT || + _msg.track_mode() == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK) + { + this->dataPtr->followingTarget = true; + } + this->FollowingTargetChanged(); + + return; +} + ///////////////////////////////////////////////// void GzSimPlugin::registerTypes(const char *_uri) { @@ -94,11 +128,17 @@ void GzSimPlugin::registerTypes(const char *_uri) EntityContextMenu::EntityContextMenu() : dataPtr(std::make_unique()) { + this->dataPtr->currentTrackTopic = "/gui/currently_tracked"; + this->dataPtr->node.Subscribe(this->dataPtr->currentTrackTopic, + &EntityContextMenu::OnCurrentlyTrackedSub, this); + gzmsg << "Currently tracking topic on [" + << this->dataPtr->currentTrackTopic << "]" << std::endl; + // For move to service requests this->dataPtr->moveToService = "/gui/move_to"; - // For follow service requests - this->dataPtr->followService = "/gui/follow"; + // For track topic message + this->dataPtr->trackTopic = "/gui/track"; // For remove service requests this->dataPtr->removeService = "/world/default/remove"; @@ -129,11 +169,27 @@ EntityContextMenu::EntityContextMenu() // For paste service requests this->dataPtr->pasteService = "/gui/paste"; + + this->dataPtr->trackPub = + this->dataPtr->node.Advertise(this->dataPtr->trackTopic); } ///////////////////////////////////////////////// EntityContextMenu::~EntityContextMenu() = default; +///////////////////////////////////////////////// +void EntityContextMenu::SetFollowingTarget(bool &_followingTarget) +{ + this->dataPtr->followingTarget = _followingTarget; + this->FollowingTargetChanged(); +} + +///////////////////////////////////////////////// +bool EntityContextMenu::FollowingTarget() const +{ + return this->dataPtr->followingTarget; +} + ///////////////////////////////////////////////// void EntityContextMenu::OnRemove( const QString &_data, const QString &_type) @@ -196,9 +252,40 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) } else if (request == "follow") { - msgs::StringMsg req; - req.set_data(_data.toStdString()); - this->dataPtr->node.Request(this->dataPtr->followService, req, cb); + msgs::CameraTrack followMsg; + followMsg.mutable_follow_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW); + this->dataPtr->followTargetLookAt = followMsg.follow_target().name(); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "free_look") + { + msgs::CameraTrack followMsg; + followMsg.mutable_follow_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_FREE_LOOK); + this->dataPtr->followTargetLookAt = followMsg.follow_target().name(); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "look_at") + { + msgs::CameraTrack followMsg; + followMsg.mutable_track_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_LOOK_AT); + followMsg.mutable_follow_target()->set_name( + this->dataPtr->followTargetLookAt); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + gzmsg << "Look at target: " << followMsg.track_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "track") + { + msgs::CameraTrack trackMsg; + trackMsg.mutable_track_target()->set_name(_data.toStdString()); + trackMsg.set_track_mode(msgs::CameraTrack::TRACK); + gzmsg << "Track target: " << trackMsg.track_target().name() << std::endl; + this->dataPtr->trackPub.Publish(trackMsg); } else if (request == "view_transparent") { diff --git a/src/gui/plugins/modules/EntityContextMenu.hh b/src/gui/plugins/modules/EntityContextMenu.hh index 381417f61e..5f09419f32 100644 --- a/src/gui/plugins/modules/EntityContextMenu.hh +++ b/src/gui/plugins/modules/EntityContextMenu.hh @@ -19,6 +19,7 @@ #define GZ_SIM_GUI_ENTITYCONTEXTMENU_HH_ #include +#include #include #include @@ -46,6 +47,13 @@ namespace sim class EntityContextMenu : public QQuickItem { Q_OBJECT + /// \brief followingTarget + Q_PROPERTY( + bool followingTarget + READ FollowingTarget + WRITE SetFollowingTarget + NOTIFY FollowingTargetChanged + ) /// \brief Constructor public: EntityContextMenu(); @@ -53,6 +61,21 @@ namespace sim /// \brief Destructor public: ~EntityContextMenu() override; + /// \brief Get whether it is following target + /// \return True if followingTarget + public: Q_INVOKABLE bool FollowingTarget() const; + + /// \brief Set whether followingTarget + /// \param[in] _followingTarget True if followingTarget + public: Q_INVOKABLE void SetFollowingTarget(bool &_followingTarget); + + /// \brief Notify that followingTarget has changed + signals: void FollowingTargetChanged(); + + /// \brief Callback function to get data from the message + /// \param[in] _msg CameraTrack message + public: void OnCurrentlyTrackedSub(const msgs::CameraTrack &_msg); + /// \brief Callback when a context menu item is invoked /// \param[in] _data Request data /// \param[in] _type Entity type diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index 1d3cd66120..72626d3f95 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -28,9 +28,19 @@ Item { onTriggered: context.OnRequest("move_to", context.entity) } MenuItem { - id: followMenu - text: "Follow" - onTriggered: context.OnRequest("follow", context.entity) + id: followOptionsSubmenu + text: "Follow Options >" + MouseArea { + id: followOptionsSubMouseArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondMenu.open() + } + } + MenuItem { + id: trackMenu + text: "Track" + onTriggered: context.OnRequest("track", context.entity) } MenuItem { id: removeMenu @@ -67,13 +77,42 @@ Item { id: viewSubMouseArea anchors.fill: parent hoverEnabled: true - onEntered: secondMenu.open() + onEntered: thirdMenu.open() } } } Menu { id: secondMenu x: menu.x + menu.width + y: menu.y + followOptionsSubmenu.y + MenuItem { + id: followMenu + text: "Follow" + onTriggered: { + menu.close() + context.OnRequest("follow", context.entity) + } + } + MenuItem { + id: followFreeLookMenu + text: "Free Look" + onTriggered: { + menu.close() + context.OnRequest("free_look", context.entity) + } + } + MenuItem { + id: followLookAtMenu + text: "Look At" + onTriggered: { + menu.close() + context.OnRequest("look_at", context.entity) + } + } + } + Menu { + id: thirdMenu + x: menu.x + menu.width y: menu.y + viewSubmenu.y MenuItem { id: viewCOMMenu @@ -140,6 +179,9 @@ Item { context.type = _type moveToMenu.enabled = false followMenu.enabled = false + followFreeLookMenu.enabled = false + followLookAtMenu.enabled = false + trackMenu.enabled = false removeMenu.enabled = false viewTransparentMenu.enabled = false; viewCOMMenu.enabled = false; @@ -156,6 +198,12 @@ Item { { moveToMenu.enabled = true followMenu.enabled = true + followFreeLookMenu.enabled = true + if (context.followingTarget) + { + followLookAtMenu.enabled = true + } + trackMenu.enabled = true } if (context.type == "model" || context.type == "light") diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 82e1492853..c5d24bc7ab 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -1245,7 +1245,14 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); - geom = this->scene->CreateMesh(descriptor); + if (descriptor.mesh) + { + geom = this->scene->CreateMesh(descriptor); + } + else + { + gzerr << "Failed to load mesh: " << descriptor.meshName << std::endl; + } scale = _geom.MeshShape()->Scale(); } else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP) diff --git a/src/network/NetworkConfig_TEST.cc b/src/network/NetworkConfig_TEST.cc index d68be8ab33..af6baffc61 100644 --- a/src/network/NetworkConfig_TEST.cc +++ b/src/network/NetworkConfig_TEST.cc @@ -33,6 +33,7 @@ TEST(NetworkManager, ValueConstructor) assert(config.role == NetworkRole::None); assert(config.numSecondariesExpected == 0); // Expect console warning as well + (void) config; } { @@ -40,23 +41,27 @@ TEST(NetworkManager, ValueConstructor) auto config = NetworkConfig::FromValues("PRIMARY", 3); assert(config.role == NetworkRole::SimulationPrimary); assert(config.numSecondariesExpected == 3); + (void) config; } { // Secondary is always valid auto config = NetworkConfig::FromValues("SECONDARY", 0); assert(config.role == NetworkRole::SimulationSecondary); + (void) config; } { // Readonly is always valid auto config = NetworkConfig::FromValues("READONLY"); assert(config.role == NetworkRole::ReadOnly); + (void) config; } { // Anything else is invalid auto config = NetworkConfig::FromValues("READ_WRITE"); assert(config.role == NetworkRole::None); + (void) config; } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index ab5d0ee92b..d94e6aa8b7 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -813,8 +813,14 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, rendering::MeshDescriptor descriptor; descriptor.meshName = name; descriptor.mesh = meshManager->MeshByName(name); - - geom = this->dataPtr->scene->CreateMesh(descriptor); + if (descriptor.mesh) + { + geom = this->dataPtr->scene->CreateMesh(descriptor); + } + else + { + gzerr << "Unable to find the polyline mesh: " << name << std::endl; + } } else { diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index d75a20663d..078b891319 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -72,7 +72,7 @@ function(gz_add_system system_name) endif() # Note that plugins are currently being installed in 2 places. /lib and the plugin dir - install(TARGETS ${system_target} DESTINATION ${GZ_SIM_PLUGIN_INSTALL_DIR}) + install(TARGETS ${system_target} DESTINATION ${GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR}) # The library created by `gz_add_component` includes the gz-sim version # (i.e. libgz-sim1-name-system.so), but for portability of SDF @@ -90,7 +90,7 @@ function(gz_add_system system_name) else() file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") - INSTALL(FILES ${PROJECT_BINARY_DIR}/lib/${unversioned} DESTINATION ${GZ_SIM_PLUGIN_INSTALL_DIR}) + INSTALL(FILES ${PROJECT_BINARY_DIR}/lib/${unversioned} DESTINATION ${GZ_SIM_PLUGIN_RELATIVE_INSTALL_DIR}) endif() endfunction() @@ -131,6 +131,7 @@ add_subdirectory(kinetic_energy_monitor) add_subdirectory(label) add_subdirectory(lens_flare) add_subdirectory(lift_drag) +add_subdirectory(lighter_than_air_dynamics) add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 3fe8a5f60d..f01c72dfb9 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -608,70 +608,29 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, for (Entity joint : this->dataPtr->leftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftJointSpeed}); } for (Entity joint : this->dataPtr->rightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightJointSpeed}); } } // Update steering for (Entity joint : this->dataPtr->leftSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->leftSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->leftSteeringJointSpeed}); } for (Entity joint : this->dataPtr->rightSteeringJoints) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd( - {this->dataPtr->rightSteeringJointSpeed}); - } + _ecm.SetComponentData( + joint, {this->dataPtr->rightSteeringJointSpeed}); } if (!this->dataPtr->steeringOnly) { @@ -926,11 +885,10 @@ void AckermannSteeringPrivate::UpdateVelocity( double leftDelta = leftSteeringJointAngle - leftSteeringPos->Data()[0]; double rightDelta = rightSteeringJointAngle - rightSteeringPos->Data()[0]; - // Simple proportional control with a gain of 1 + // Simple proportional control with settable gain. // Adding programmable PID values might be a future feature. - // Works as is for tested cases - this->leftSteeringJointSpeed = leftDelta; - this->rightSteeringJointSpeed = rightDelta; + this->leftSteeringJointSpeed = this->gainPAng * leftDelta; + this->rightSteeringJointSpeed = this->gainPAng * rightDelta; } ////////////////////////////////////////////////// @@ -968,11 +926,11 @@ void AckermannSteeringPrivate::UpdateAngle( double leftSteeringJointAngle = atan((2.0 * this->wheelBase * sin(ang)) / \ - ((2.0 * this->wheelBase * cos(ang)) + \ + ((2.0 * this->wheelBase * cos(ang)) - \ (1.0 * this->wheelSeparation * sin(ang)))); double rightSteeringJointAngle = atan((2.0 * this->wheelBase * sin(ang)) / \ - ((2.0 * this->wheelBase * cos(ang)) - \ + ((2.0 * this->wheelBase * cos(ang)) + \ (1.0 * this->wheelSeparation * sin(ang)))); auto leftSteeringPos = _ecm.Component( diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 3d829d8902..84583379c4 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -52,7 +52,6 @@ namespace systems /// the index of the steering angle position actuator. /// /// - ``: Float used to control the steering angle P gain. - /// Only used for when in steering_only mode. /// /// - ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 700e342743..04fc968279 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -459,17 +459,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, continue; // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->leftJointSpeed}); } for (Entity joint : this->dataPtr->rightJoints) @@ -479,17 +470,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, continue; // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->rightJointSpeed}); } // Create the left and right side joint position components if they diff --git a/src/systems/dvl/DopplerVelocityLogSystem.cc b/src/systems/dvl/DopplerVelocityLogSystem.cc index 103311ed6b..fbf5d7d873 100644 --- a/src/systems/dvl/DopplerVelocityLogSystem.cc +++ b/src/systems/dvl/DopplerVelocityLogSystem.cc @@ -220,6 +220,9 @@ class gz::sim::systems::DopplerVelocityLogSystem::Implementation /// \brief Current simulation time. public: std::chrono::steady_clock::duration nextUpdateTime{ std::chrono::steady_clock::duration::max()}; + + /// \brief Mutex to protect current simulation times + public: std::mutex timeMutex; }; using namespace gz; @@ -351,6 +354,8 @@ void DopplerVelocityLogSystem::Implementation::DoPostUpdate( return true; }); + std::lock_guard timeLock(this->timeMutex); + if (!this->perStepRequests.empty() || ( !_info.paused && this->nextUpdateTime <= _info.simTime)) { @@ -611,6 +616,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender() } auto closestUpdateTime = std::chrono::steady_clock::duration::max(); + + std::lock_guard timeLock(this->timeMutex); + for (const auto & [_, sensorId] : this->sensorIdPerEntity) { gz::sensors::Sensor *sensor = @@ -635,6 +643,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender() void DopplerVelocityLogSystem::Implementation::OnPostRender() { GZ_PROFILE("DopplerVelocityLogSystem::Implementation::OnPostRender"); + + std::lock_guard timeLock(this->timeMutex); + for (const auto & sensorId : this->updatedSensorIds) { auto *sensor = diff --git a/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp index e4fe1fa821..7b5843c009 100644 --- a/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp @@ -525,7 +525,8 @@ class state_transition_table { void check_default_transition() { - auto const& ttable = transition_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& ttable = transition_table(st); ttable[current_state()](*this, none{}); } @@ -544,7 +545,8 @@ class state_transition_table { void exit(Event&& event) { - auto const& table = exit_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& table = exit_table(st); table[current_state()](states_, ::std::forward(event), *fsm_); } @@ -552,7 +554,8 @@ class state_transition_table { actions::event_process_result process_transition_event(Event&& event) { - auto const& inv_table = transition_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& inv_table = transition_table(st); return inv_table[current_state()](*this, ::std::forward(event)); } @@ -655,10 +658,11 @@ class state_transition_table { event_set current_handled_events() const { - auto const& table = get_current_events_table(state_indexes{}); + auto st = state_indexes{}; + auto const& table = get_current_events_table(st); auto res = table[current_state_](states_); auto const& available_transitions - = get_available_transitions_table(state_indexes{}); + = get_available_transitions_table(st); auto const& trans = available_transitions[current_state_]; res.insert( trans.begin(), trans.end()); return res; @@ -667,7 +671,8 @@ class state_transition_table { event_set current_deferrable_events() const { - auto const& table = get_current_deferred_events_table(state_indexes{}); + auto st = state_indexes{}; + auto const& table = get_current_deferred_events_table(st); return table[current_state_](states_); } diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 652c793265..71a2e2800b 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -181,10 +181,10 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, // note: gz-sensors does its own throttling. Here the check is mainly // to avoid doing work in the ForceTorquePrivate::Update function bool needsUpdate = false; - for (auto &it : this->dataPtr->entitySensorMap) + for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - if (it.second->NextDataUpdateTime() <= _info.simTime && - it.second->HasConnections()) + if (sensor->NextDataUpdateTime() <= _info.simTime && + sensor->HasConnections()) { needsUpdate = true; break; @@ -193,11 +193,16 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, if (!needsUpdate) return; + // Transform joint wrench to sensor wrench and write to sensor this->dataPtr->Update(_ecm); - for (auto &it : this->dataPtr->entitySensorMap) + for (auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - it.second->Update(_info.simTime, false); + // Call gz::sensors::ForceTorqueSensor::Update + // * Convert to user-specified frame + // * Apply noise + // * Publish to gz-transport topic + sensor->Update(_info.simTime, false); } } @@ -269,7 +274,8 @@ void ForceTorquePrivate::Update(const EntityComponentManager &_ecm) return true; } - // Appropriate components haven't been populated by physics yet + // Return early if JointTransmittedWrench component has not yet been + // populated by the Physics system auto jointWrench = _ecm.Component( jointLinkIt->second.joint); if (nullptr == jointWrench) diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 4c18ac7bdf..40f5caac33 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -344,17 +344,7 @@ void JointController::PreUpdate(const UpdateInfo &_info, // Update joint velocity for (Entity joint : this->dataPtr->jointEntities) { - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } } } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 80d36c2f9e..b2835160ff 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -461,17 +461,7 @@ void JointPositionController::PreUpdate( for (Entity joint : this->dataPtr->jointEntities) { // Update velocity command. - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({targetVel})); - } - else - { - *vel = components::JointVelocityCmd({targetVel}); - } + _ecm.SetComponentData(joint, {targetVel}); } return; } diff --git a/src/systems/lighter_than_air_dynamics/CMakeLists.txt b/src/systems/lighter_than_air_dynamics/CMakeLists.txt new file mode 100644 index 0000000000..71e42f7f36 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(lighter_than_air_dynamics + SOURCES + LighterThanAirDynamics.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-math${GZ_MATH_VER}::eigen3 +) diff --git a/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc new file mode 100644 index 0000000000..c895235e22 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc @@ -0,0 +1,464 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/Environment.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/components/Wind.hh" +#include "gz/sim/components/Inertial.hh" + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/System.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" + +#include + +#include "gz/math/Matrix3.hh" + +#include "gz/transport/Node.hh" + +#include +#include + +#include "LighterThanAirDynamics.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private LighterThanAirDynamics data class. +class gz::sim::systems::LighterThanAirDynamicsPrivateData +{ + + /// \brief air density [kg/m^3]. + public: double airDensity; + + /// \brief force coefficient of the viscous flow contribution to the hull + public: double forceViscCoeff; + + /// \brief force coefficient of the inviscid flow contribution to the hull + public: double forceInviscCoeff; + + /// \brief moment coefficient of the viscous flow contribution to the hull + public: double momentViscCoeff; + + /// \brief moment coefficient of the inviscid flow contribution to the hull + public: double momentInviscCoeff; + + /// \brief Top left [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m11; + + /// \brief Top right [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m12; + + /// \brief Bottom left [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m21; + + /// \brief Bottom right [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m22; + + /// \brief distance measured from the nose, where the flow stopped + /// being potential + public: double epsV; + + /// \brief axial drag coefficient of the hull + public: double axialDragCoeff; + + /// \brief The Gazebo Transport node + public: transport::Node node; + + /// \brief Link entity + public: Entity linkEntity; + + /// \brief World frame wind + public: math::Vector3d windVector {0, 0, 0}; + + /// \brief wind current callback + public: void UpdateWind(const msgs::Vector3d &_msg); + + /// \brief Mutex + public: std::mutex mtx; +}; + +///////////////////////////////////////////////// +void LighterThanAirDynamicsPrivateData::UpdateWind(const msgs::Vector3d &_msg) +{ + std::lock_guard lock(this->mtx); + this->windVector = gz::msgs::Convert(_msg); +} + +///////////////////////////////////////////////// +void AddAngularVelocityComponent( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::AngularVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::WorldAngularVelocity()); + } +} + +///////////////////////////////////////////////// +void AddWorldPose( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, gz::sim::components::WorldPose()); + } +} + +///////////////////////////////////////////////// +void AddWorldInertial( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, gz::sim::components::Inertial()); + } +} + +///////////////////////////////////////////////// +void AddWorldLinearVelocity( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::WorldLinearVelocity()); + } +} + +///////////////////////////////////////////////// +double SdfParamDouble( + const std::shared_ptr &_sdf, + const std::string& _field, + double _default) +{ + return _sdf->Get(_field, _default).first; +} + +math::Matrix3d LighterThanAirDynamics::SkewSymmetricMatrix(math::Vector3d mat) +{ + math::Matrix3d skewSymmetric(0, -1.0*mat[2], mat[1], + mat[2], 0, -1.0*mat[0], + -1.0*mat[1], mat[0], 0); + + + return skewSymmetric; +} + + +math::Vector3d LighterThanAirDynamics::AddedMassForce( + math::Vector3d lin_vel, math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12) +{ + auto skewAngVel = this->SkewSymmetricMatrix(ang_vel); + math::Vector3d forces = skewAngVel * (m11 * lin_vel + m12 * ang_vel); + + return forces; +} + +math::Vector3d LighterThanAirDynamics::AddedMassTorque( + math::Vector3d lin_vel, math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, math::Matrix3d m21, + math::Matrix3d m22) +{ + auto skewAngVel = this->SkewSymmetricMatrix(ang_vel); + auto skewLinVel = this->SkewSymmetricMatrix(lin_vel); + + // note: the m11*lin_vel term: it is already accounted in the + // inviscous term see [2], and thus removed by the zero multiplication, + // but is added here for visibility. + math::Vector3d torque = skewLinVel * (m11 * lin_vel*0 + m12 * ang_vel) + + skewAngVel * (m21 * lin_vel + m22 * ang_vel); + + return torque; +} + +math::Vector3d LighterThanAirDynamics::LocalVelocity(math::Vector3d lin_vel, + math::Vector3d ang_vel, math::Vector3d dist) +{ + return lin_vel + ang_vel.Cross(dist); +} + +double LighterThanAirDynamics::DynamicPressure( + math::Vector3d vec, double air_density) +{ + return 0.5 * air_density * vec.SquaredLength(); +} + +///////////////////////////////////////////////// +LighterThanAirDynamics::LighterThanAirDynamics() +{ + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +LighterThanAirDynamics::~LighterThanAirDynamics() +{ + // Do nothing +} + +///////////////////////////////////////////////// +void LighterThanAirDynamics::Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/ +) +{ + if (_sdf->HasElement("air_density")) + { + this->dataPtr->airDensity = SdfParamDouble(_sdf, "air_density", 1.225); + } + + // Create model object, to access convenient functions + auto model = gz::sim::Model(_entity); + + if (!_sdf->HasElement("link_name")) + { + gzerr << "You must specify a for the lighter than air" + << " plugin to act upon"; + return; + } + auto linkName = _sdf->Get("link_name"); + this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName); + if (!_ecm.HasEntity(this->dataPtr->linkEntity)) + { + gzerr << "Link name" << linkName << "does not exist"; + return; + } + + if(_sdf->HasElement("moment_inviscid_coeff")) + { + this->dataPtr->momentInviscCoeff = + _sdf->Get("moment_inviscid_coeff"); + + gzdbg << "moment_inviscid_coeff: " + << this->dataPtr->momentInviscCoeff << "\n"; + }else{ + gzerr << "moment_inviscid_coeff not found \n"; + } + + if(_sdf->HasElement("moment_viscous_coeff")) + { + this->dataPtr->momentViscCoeff = _sdf->Get("moment_viscous_coeff"); + gzdbg << "moment_viscous_coeff: " + << this->dataPtr->momentViscCoeff << "\n"; + }else{ + gzerr << "moment_viscous_coeff not found \n"; + return; + } + + if(_sdf->HasElement("force_inviscid_coeff")) + { + this->dataPtr->forceInviscCoeff = + _sdf->Get("force_inviscid_coeff"); + + gzdbg << "force_inviscid_coeff: " + << this->dataPtr->forceInviscCoeff << "\n"; + }else{ + gzerr << "force_inviscid_coeff not found \n"; + return; + } + + if(_sdf->HasElement("force_viscous_coeff")) + { + this->dataPtr->forceViscCoeff = _sdf->Get("force_viscous_coeff"); + + gzdbg << "force_inviscous_coeff: " << this->dataPtr->forceViscCoeff << "\n"; + }else{ + gzerr << "force_inviscous_coeff not found \n"; + return; + } + + if(_sdf->HasElement("eps_v")) + { + this->dataPtr->epsV = _sdf->Get("eps_v"); + gzdbg << "eps_v: " << this->dataPtr->epsV << "\n"; + }else{ + gzerr << "eps_v not found \n"; + return; + } + + if(_sdf->HasElement("axial_drag_coeff")) + { + this->dataPtr->axialDragCoeff = _sdf->Get("axial_drag_coeff"); + gzdbg << "axial_drag_coeff: " << this->dataPtr->axialDragCoeff << "\n"; + }else{ + gzerr << "axial_drag_coeff not found \n"; + return; + } + + AddWorldPose(this->dataPtr->linkEntity, _ecm); + AddWorldInertial(this->dataPtr->linkEntity, _ecm); + AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm); + AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm); + + gz::sim::Link baseLink(this->dataPtr->linkEntity); + + math::Matrix6d added_mass_matrix = + (baseLink.WorldFluidAddedMassMatrix(_ecm)).value(); + + this->dataPtr->m11 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::TOP_LEFT); + this->dataPtr->m12 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::TOP_RIGHT); + this->dataPtr->m21 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::BOTTOM_LEFT); + this->dataPtr->m22 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::BOTTOM_RIGHT); +} + +///////////////////////////////////////////////// +void LighterThanAirDynamics::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (_info.paused) + return; + + // Get vehicle state + gz::sim::Link baseLink(this->dataPtr->linkEntity); + auto linearVelocity = _ecm.Component( + this->dataPtr->linkEntity); + auto rotationalVelocity = baseLink.WorldAngularVelocity(_ecm); + + if (!linearVelocity) + { + gzerr << "no linear vel" <<"\n"; + return; + } + + // Transform state to local frame + auto pose = baseLink.WorldPose(_ecm); + // Since we are transforming angular and linear velocity we only care about + // rotation. Also this is where we apply the effects of current to the link + auto linearVel = pose->Rot().Inverse() * (linearVelocity->Data()); + auto angularVelocity = pose->Rot().Inverse() * *rotationalVelocity; + + // Calculate viscous forces + math::Vector3d velEpsV = LocalVelocity(linearVel, angularVelocity, + math::Vector3d(-this->dataPtr->epsV, 0, 0)); + + double q0EpsV = DynamicPressure(velEpsV, this->dataPtr->airDensity); + double gammaEpsV = 0.0f; + + gammaEpsV = std::atan2(std::sqrt(velEpsV[1]*velEpsV[1] + + velEpsV[2]*velEpsV[2]), velEpsV[0]); + + double forceInviscCoeff = this->dataPtr->forceInviscCoeff; + double forceViscCoeff = this->dataPtr->forceViscCoeff; + double momentInviscCoeff = this->dataPtr->momentInviscCoeff; + double momentViscCoeff = this->dataPtr->momentViscCoeff; + + double forceViscMag_ = q0EpsV*(-forceInviscCoeff*std::sin(2*gammaEpsV) + + forceViscCoeff*std::sin(gammaEpsV)*std::sin(gammaEpsV)); + + double momentViscMag_ = q0EpsV*(-momentInviscCoeff*std::sin(2*gammaEpsV) + + momentViscCoeff*std::sin(gammaEpsV)*std::sin(gammaEpsV)); + + double viscNormalMag_ = std::sqrt(velEpsV[1]*velEpsV[1] + + velEpsV[2]*velEpsV[2]); + + double viscNormalY_ = 0.0; + double viscNormalZ_ = 0.0; + + if(viscNormalMag_ > std::numeric_limits::epsilon()){ + + viscNormalY_ = velEpsV[1]/viscNormalMag_; + viscNormalZ_ = velEpsV[2]/viscNormalMag_; + } + + auto forceVisc = forceViscMag_ * math::Vector3d(0, + -viscNormalY_, + -viscNormalZ_); + + auto momentVisc = momentViscMag_ * math::Vector3d(0, + viscNormalZ_, + -viscNormalY_); + + // Added Mass forces & Torques + auto forceAddedMass = -1.0 * this->AddedMassForce(linearVel, + angularVelocity, + this->dataPtr->m11, + this->dataPtr->m12); + + auto momentAddedMass = -1.0 * this->AddedMassTorque(linearVel, + angularVelocity, + this->dataPtr->m11, + this->dataPtr->m12, + this->dataPtr->m21, + this->dataPtr->m22); + + // Axial drag + double q0 = DynamicPressure(linearVel, this->dataPtr->airDensity); + double angleOfAttack = std::atan2(linearVel[2], linearVel[0]); + double axialDragCoeff = this->dataPtr->axialDragCoeff; + + auto forceAxialDrag = math::Vector3d(-q0 * axialDragCoeff * + std::cos(angleOfAttack) * std::cos(angleOfAttack), 0, 0); + + math::Vector3d totalForce = forceAddedMass + forceVisc + forceAxialDrag; + math::Vector3d totalTorque = momentAddedMass + momentVisc; + + baseLink.AddWorldWrench(_ecm, + pose->Rot()*(totalForce), + pose->Rot()*totalTorque); +} + +GZ_ADD_PLUGIN( + LighterThanAirDynamics, System, + LighterThanAirDynamics::ISystemConfigure, + LighterThanAirDynamics::ISystemPreUpdate +) + +GZ_ADD_PLUGIN_ALIAS( + LighterThanAirDynamics, + "gz::sim::systems::LighterThanAirDynamics") + +// TODO(CH3): Deprecated, remove on version 8 +GZ_ADD_PLUGIN_ALIAS( + LighterThanAirDynamics, + "ignition::gazebo::systems::LighterThanAirDynamics") diff --git a/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh new file mode 100644 index 0000000000..a7c0971d55 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_SYSTEMS_LighterThanAirDynamics_HH_ +#define GZ_SIM_SYSTEMS_LighterThanAirDynamics_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + class LighterThanAirDynamicsPrivateData; + + /// \brief This class provides the effect of viscousity on the hull of + /// lighter-than-air vehicles such as airships. The equations implemented + /// is based on the work published in [1], which describes a modeling + /// approach for the nonlinear dynamics simulation of airships and [2] + /// providing more insight of the modelling of an airship. + /// + /// ## System Parameters + /// * `` sets the density of air that surrounds + /// the buoyant object. [Units: kgm^-3] + + /// * `` is the second coefficient in Eq (11) in [1]: + /// \f$ \eta C_{DC} \cdot \int_{\varepsilon_{0}}^{L} 2R \,d\varepsilon \f$ + + /// * `` is the second coefficient in Eq (14) in [1]: + /// \f$ \eta C_{DC} \cdot \int_{\varepsilon_{0}}^{L} 2R (\varepsilon_{m} - + /// \varepsilon) \,d\varepsilon \f$ + + /// * `` is the point on the hull where the flow ceases being + /// potential + + /// * `` the actual drag coefficient of the hull + + /// ## Notes + /// + /// This class only implements the viscous effects on the hull of an + /// airship and currently does not take into the account wind. + /// This class should be used in combination with the boyuancy, added mass + /// and gravity plugins to simulate the behaviour of an airship. + /// Its important to provide a collision property to the hull, since this is + /// from which the buoyancy plugin determines the volume. + /// + /// # Citations + /// [1] Li, Y., & Nahon, M. (2007). Modeling and simulation of airship + /// dynamics. Journal of Guidance, Control, and Dynamics, 30(6), 1691–1700. + /// + /// [2] Li, Y., Nahon, M., & Sharf, I. (2011). Airship dynamics modeling: + /// A literature review. Progress in Aerospace Sciences, 47(3), 217–239. + + class LighterThanAirDynamics: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate + { + /// \brief Constructor + public: LighterThanAirDynamics(); + + /// \brief Destructor + public: ~LighterThanAirDynamics() override; + + /// Documentation inherited + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/) override; + + /// Documentation inherited + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + ///////////////////////////////////////////////// + /// \brief Calculates the local velocity at an offset from a origin + /// \param[in] lin_vel - The linear body velocity + /// \param[in] ang_vel - The angular body velocity + /// \param[in] dist - The distance vector from the origin + /// \return The local velocity at the distance vector + public: math::Vector3d LocalVelocity(math::Vector3d lin_vel, + math::Vector3d ang_vel, math::Vector3d dist); + + ///////////////////////////////////////////////// + /// \brief Calculates dynamic pressure + /// \param[in] vec - The linear velocity + /// \param[in] air_density - The air density [kg/m^3] + /// \return The dynamic pressure, q + public: double DynamicPressure(math::Vector3d vec, double air_density); + + ///////////////////////////////////////////////// + /// \brief Calculates the potential flow aerodynamic forces that a LTA + /// vehicle experience when moving in a potential fluid. The aerodynamic + /// force is derived using Kirchoff's equation. + /// \param[in] lin_vel - The body linear velocity + /// \param[in] ang_vel - The body angular velocity + /// \param[in] m11 - The left upper [3x3] matrix of the added mass matrix + /// \param[in] m12 - The right upper [3x3] matrix of the added mass matrix + /// \param[in] m21 - The left lower [3x3] matrix of the added mass matrix + /// \param[in] m22 - The right lower [3x3] matrix of the added mass matrix + /// \return The aerodynamic force. + public: math::Vector3d AddedMassTorque(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, + math::Matrix3d m21, math::Matrix3d m22); + + ///////////////////////////////////////////////// + /// \brief Calculates the potential flow aerodynamic torques that a LTA + /// vehicle experience when moving in a potential fluid. The aerodynamic + /// torques is derived using Kirchoff's equation. + /// \param[in] lin_vel - The body linear velocity + /// \param[in] ang_vel - The body angular velocity + /// \param[in] m11 - The left upper [3x3] matrix of the added mass matrix + /// \param[in] m12 - The right upper [3x3] matrix of the added mass matrix + /// \return The aerodynamic torque + public: math::Vector3d AddedMassForce(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12); + + ///////////////////////////////////////////////// + /// \brief Skew-symmetric matrices can be used to represent cross products + /// as matrix multiplications. + /// \param[in] mat - A [3x1] vector + /// \return The skew-symmetric matrix of mat + public: math::Matrix3d SkewSymmetricMatrix(math::Vector3d mat); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 6d77de4c77..2f585307c7 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -426,66 +426,29 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, for (Entity joint : this->dataPtr->frontLeftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontLeftJointSpeed}); } for (Entity joint : this->dataPtr->frontRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed})); - } - else - { - *vel = - components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->frontRightJointSpeed}); } for (Entity joint : this->dataPtr->backLeftJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backLeftJointSpeed}); } for (Entity joint : this->dataPtr->backRightJoints) { // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->backRightJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); - } + _ecm.SetComponentData(joint, + {this->dataPtr->backRightJointSpeed}); } } diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 8e6e421ce9..12dcab4c86 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -447,14 +447,6 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, doUpdateForcesAndMoments = false; } - if (!_ecm.Component( - this->dataPtr->jointEntity)) - { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointVelocityCmd({0})); - doUpdateForcesAndMoments = false; - } - if (!_ecm.Component(this->dataPtr->linkEntity)) { _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose()); @@ -682,11 +674,10 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( refMotorRotVel = this->rotorVelocityFilter->UpdateFilter( this->refMotorInput, this->samplingTime); - const auto jointVelCmd = _ecm.Component( - this->jointEntity); - *jointVelCmd = components::JointVelocityCmd( - {this->turningDirection * refMotorRotVel - / this->rotorVelocitySlowdownSim}); + _ecm.SetComponentData( + this->jointEntity, + {this->turningDirection * refMotorRotVel + / this->rotorVelocitySlowdownSim}); } } } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 9c989f309d..7b59091d74 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3651,12 +3651,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); - _ecm.Each( - [&](const Entity &, components::JointVelocityCmd *_vel) -> bool - { - std::fill(_vel->Data().begin(), _vel->Data().end(), 0.0); - return true; - }); + { + std::vector entitiesJointVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::JointVelocityCmd *) -> bool + { + entitiesJointVelocityCmd.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesJointVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } _ecm.Each( [&](const Entity &, components::SlipComplianceCmd *_slip) -> bool @@ -3664,21 +3672,37 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, std::fill(_slip->Data().begin(), _slip->Data().end(), 0.0); return true; }); - GZ_PROFILE_END(); - _ecm.Each( - [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool - { - _vel->Data() = math::Vector3d::Zero; - return true; - }); + { + std::vector entitiesAngularVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::AngularVelocityCmd *) -> bool + { + entitiesAngularVelocityCmd.push_back(_entity); + return true; + }); - _ecm.Each( - [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool - { - _vel->Data() = math::Vector3d::Zero; - return true; - }); + for (const auto entity : entitiesAngularVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } + + { + std::vector entitiesLinearVelocityCmd; + _ecm.Each( + [&](const Entity &_entity, components::LinearVelocityCmd *) -> bool + { + entitiesLinearVelocityCmd.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesLinearVelocityCmd) + { + _ecm.RemoveComponent(entity); + } + } + GZ_PROFILE_END(); // Update joint positions GZ_PROFILE_BEGIN("Joints"); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 3ce3f4b619..84769ca3f2 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -41,6 +40,7 @@ #include #include #include +#include #include #include @@ -228,6 +228,9 @@ class gz::sim::systems::SensorsPrivate /// \brief Check if any of the sensors have connections public: bool SensorsHaveConnections(); + /// \brief Returns all sensors that have a pending trigger + public: std::unordered_set SensorsWithPendingTrigger(); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -745,11 +748,15 @@ void Sensors::PostUpdate(const UpdateInfo &_info, this->dataPtr->sensorsToUpdate, _info.simTime); } + std::unordered_set sensorsWithPendingTriggers = + this->dataPtr->SensorsWithPendingTrigger(); + // notify the render thread if updates are available if (hasRenderConnections || this->dataPtr->nextUpdateTime <= _info.simTime || this->dataPtr->renderUtil.PendingSensors() > 0 || - this->dataPtr->forceUpdate) + this->dataPtr->forceUpdate || + !sensorsWithPendingTriggers.empty()) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); @@ -769,6 +776,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lockSensors(this->dataPtr->sensorsMutex); this->dataPtr->activeSensors = std::move(this->dataPtr->sensorsToUpdate); + // Add all sensors that have pending triggers. + this->dataPtr->activeSensors.insert(sensorsWithPendingTriggers.begin(), + sensorsWithPendingTriggers.end()); } this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( @@ -1075,6 +1085,27 @@ bool SensorsPrivate::SensorsHaveConnections() return false; } +////////////////////////////////////////////////// +std::unordered_set +SensorsPrivate::SensorsWithPendingTrigger() +{ + std::unordered_set sensorsWithPendingTrigger; + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + if (s->HasPendingTrigger()) + { + sensorsWithPendingTrigger.insert(id); + } + } + return sensorsWithPendingTrigger; +} + GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemReset, diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index e7663924b2..87aa50dcb4 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -740,18 +740,8 @@ void Thruster::PreUpdate( // Velocity control else { - auto velocityComp = - _ecm.Component( - this->dataPtr->jointEntity); - if (velocityComp == nullptr) - { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointVelocityCmd({desiredPropellerAngVel})); - } - else - { - velocityComp->Data()[0] = desiredPropellerAngVel; - } + _ecm.SetComponentData( + this->dataPtr->jointEntity, {desiredPropellerAngVel}); angvel.set_data(desiredPropellerAngVel); } diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc index 1db5e1c2d0..e295686946 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.cc +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -111,9 +111,6 @@ class gz::sim::systems::TrajectoryFollowerPrivate /// \brief Whether the trajectory follower behavior should be paused or not. public: bool paused = false; - /// \brief Angular velocity set to zero - public: bool zeroAngVelSet = false; - /// \brief Force angular velocity to be zero when bearing is reached public: bool forceZeroAngVel = false; }; @@ -390,37 +387,22 @@ void TrajectoryFollower::PreUpdate( // Transform the force and torque to the world frame. // Move commands. The vehicle always move forward (X direction). gz::math::Vector3d forceWorld; + gz::math::Vector3d torqueWorld; if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance) { forceWorld = (*comPose).Rot().RotateVector( gz::math::Vector3d(this->dataPtr->forceToApply, 0, 0)); // force angular velocity to be zero when bearing is reached - if (this->dataPtr->forceZeroAngVel && !this->dataPtr->zeroAngVelSet && + if (this->dataPtr->forceZeroAngVel && math::equal (std::abs(bearing.Degree()), 0.0, this->dataPtr->bearingTolerance * 0.5)) { this->dataPtr->link.SetAngularVelocity(_ecm, math::Vector3d::Zero); - this->dataPtr->zeroAngVelSet = true; } } - gz::math::Vector3d torqueWorld; - if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance) + else { - // remove angular velocity component otherwise the physics system will set - // the zero ang vel command every iteration - if (this->dataPtr->forceZeroAngVel && this->dataPtr->zeroAngVelSet) - { - auto angVelCmdComp = _ecm.Component( - this->dataPtr->link.Entity()); - if (angVelCmdComp) - { - _ecm.RemoveComponent( - this->dataPtr->link.Entity()); - this->dataPtr->zeroAngVelSet = false; - } - } - int sign = static_cast(std::abs(bearing.Degree()) / bearing.Degree()); torqueWorld = (*comPose).Rot().RotateVector( gz::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply)); diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 68224b3bab..b5a1ccbde1 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -197,38 +197,12 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, return; // update angular velocity of model - auto modelAngularVel = - _ecm.Component( - this->dataPtr->model.Entity()); - - if (modelAngularVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->model.Entity(), - components::AngularVelocityCmd({this->dataPtr->angularVelocity})); - } - else - { - *modelAngularVel = - components::AngularVelocityCmd({this->dataPtr->angularVelocity}); - } + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {this->dataPtr->angularVelocity}); // update linear velocity of model - auto modelLinearVel = - _ecm.Component( - this->dataPtr->model.Entity()); - - if (modelLinearVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->model.Entity(), - components::LinearVelocityCmd({this->dataPtr->linearVelocity})); - } - else - { - *modelLinearVel = - components::LinearVelocityCmd({this->dataPtr->linearVelocity}); - } + _ecm.SetComponentData( + this->dataPtr->model.Entity(), {this->dataPtr->linearVelocity}); // If there are links, create link components // If the link hasn't been identified yet, look for it @@ -262,17 +236,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, auto it = this->dataPtr->links.find(linkName); if (it != this->dataPtr->links.end()) { - auto linkAngularVelComp = - _ecm.Component(it->second); - if (!linkAngularVelComp) - { - _ecm.CreateComponent(it->second, - components::AngularVelocityCmd({angularVel})); - } - else - { - *linkAngularVelComp = components::AngularVelocityCmd(angularVel); - } + _ecm.SetComponentData( + it->second, {angularVel}); } else { @@ -286,17 +251,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, auto it = this->dataPtr->links.find(linkName); if (it != this->dataPtr->links.end()) { - auto linkLinearVelComp = - _ecm.Component(it->second); - if (!linkLinearVelComp) - { - _ecm.CreateComponent(it->second, - components::LinearVelocityCmd({linearVel})); - } - else - { - *linkLinearVelComp = components::LinearVelocityCmd(linearVel); - } + _ecm.SetComponentData( + it->second, {linearVel}); } else { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 61f350bd73..639674e4a3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -44,6 +44,7 @@ set(tests level_manager.cc level_manager_runtime_performers.cc light.cc + lighter_than_air_dynamics.cc link.cc logical_camera_system.cc logical_audio_sensor_plugin.cc diff --git a/test/integration/lighter_than_air_dynamics.cc b/test/integration/lighter_than_air_dynamics.cc new file mode 100644 index 0000000000..b2edf7cf0c --- /dev/null +++ b/test/integration/lighter_than_air_dynamics.cc @@ -0,0 +1,365 @@ +/* + * Copyright (C) 2023 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 "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/TestFixture.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/World.hh" + +#include "test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class LighterThanAirDynamicsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file + /// \param[in] _world Path to world file + /// \param[in] _namespace Namespace for topic + public: std::vector TestWorld(const std::string &_world, + const std::string &_namespace); + + public: std::vector> + TestUnstableYaw( + const std::string &_world, const std::string &_namespace); + + public: std::vector> + TestUnstablePitch( + const std::string &_world, const std::string &_namespace); + + private: std::vector worldPoses; + private: std::vector bodyAngularVels; + private: std::vector> state; +}; + + +////////////////////////////////////////////////// +std::vector> +LighterThanAirDynamicsTest::TestUnstablePitch( +const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + + worldPoses.clear(); + bodyAngularVels.clear(); + state.clear(); + + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + // Small disturbance to enduce munk moment + math::Vector3d body_z_force(0, 0, 100); + math::Vector3d world_frame_force = DCM * body_z_force; + + body.AddWorldForce(_ecm, world_frame_force); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto worldPose = body.WorldPose(_ecm); + math::Vector3d bodyAngularVel = \ + (body.WorldPose(_ecm)).value().Rot().Inverse() * \ + body.WorldAngularVelocity(_ecm).value(); + ASSERT_TRUE(worldPose); + worldPoses.push_back(worldPose.value()); + bodyAngularVels.push_back(bodyAngularVel); + state.push_back(std::make_pair(worldPose.value(), bodyAngularVel)); + }). + OnPreUpdate([&](const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) + { + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + math::Vector3d body_x_force(100, 0, 0); + + math::Vector3d world_frame_force = DCM * body_x_force; + + body.AddWorldForce(_ecm, world_frame_force); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, worldPoses.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return state; +} + +////////////////////////////////////////////////// +std::vector> + LighterThanAirDynamicsTest::TestUnstableYaw( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + worldPoses.clear(); + bodyAngularVels.clear(); + state.clear(); + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + // Small disturbance to enduce some lateral movement + // to allow munk moment to grow + math::Vector3d body_y_force(0, 10, 0); + math::Vector3d world_frame_force = DCM * body_y_force; + + body.AddWorldForce(_ecm, world_frame_force); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto worldPose = body.WorldPose(_ecm); + math::Vector3d bodyAngularVel = + (body.WorldPose(_ecm)).value().Rot().Inverse() * \ + body.WorldAngularVelocity(_ecm).value(); + ASSERT_TRUE(worldPose); + worldPoses.push_back(worldPose.value()); + bodyAngularVels.push_back(bodyAngularVel); + state.push_back(std::make_pair(worldPose.value(), bodyAngularVel)); + + }). + OnPreUpdate([&](const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) + { + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + math::Vector3d body_x_force(100, 0, 0); + + math::Vector3d world_frame_force = DCM * body_x_force; + + body.AddWorldForce(_ecm, world_frame_force); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, worldPoses.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return state; +} + +////////////////////////////////////////////////// +// Test if the plugin can be loaded succesfully and +// runs +std::vector LighterThanAirDynamicsTest::TestWorld( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + bodyVels.push_back(bodyVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, bodyVels.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return bodyVels; +} + +///////////////////////////////////////////////// +/// This test evaluates whether the lighter-than-air-dynamics plugin +/// is loaded successfully and is stable +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnModel)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + this->TestWorld(world, "hull"); + +} + +///////////////////////////////////////////////// +// Test whether the yaw of the hull will grow +// due to a a small disturbance in its lateral system +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32( + UnstableMunkMomentInYaw)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + auto states = this->TestUnstableYaw(world, "hull"); + auto states2 = this->TestUnstableYaw(world, "hull2"); + + math::Vector3d ang_vel = states.back().second; + math::Vector3d end_pos = states.back().first.Pos(); + math::Quaterniond end_rot = states.back().first.Rot(); + + math::Vector3d ang_vel2 = states2.back().second; + math::Vector3d end_pos2 = states2.back().first.Pos(); + math::Quaterniond end_rot2 = states2.back().first.Rot(); + + // Drag from hull should result in translating less than an object with no + // aerodynamic drag + EXPECT_LE(end_pos.X(), end_pos2.X()); + + // Due to the munk moment, the yaw of the aircraft should grow + EXPECT_GT(abs(end_rot.Euler().Z()), abs(end_rot2.Euler().Z())); + + // Due to the munk moment, the yawrate needs to increase + EXPECT_GT(abs(ang_vel.Z()), abs(ang_vel2.Z())); + +} + +///////////////////////////////////////////////// +// Test whether the pitch of the hull will grow +// due to a a small disturbance in its longitudinal system +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32( + UnstableMunkMomentInPitch)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + auto states = this->TestUnstablePitch(world, "hull"); + auto states2 = this->TestUnstablePitch(world, "hull2"); + + math::Vector3d ang_vel = states.back().second; + math::Vector3d end_pos = states.back().first.Pos(); + math::Quaterniond end_rot = states.back().first.Rot(); + + math::Vector3d ang_vel2 = states2.back().second; + math::Vector3d end_pos2 = states2.back().first.Pos(); + math::Quaterniond end_rot2 = states2.back().first.Rot(); + + // Drag from hull should result in translating less than an object with no + // aerodynamic drag + EXPECT_LE(end_pos.X(), end_pos2.X()); + + // Due to the munk moment, the yaw of the aircraft should grow + EXPECT_GT(abs(end_rot.Euler().Y()), abs(end_rot2.Euler().Y())); + + // Due to the munk moment, the yawrate needs to increase + EXPECT_GT(abs(ang_vel.Y()), abs(ang_vel2.Y())); + +} diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 3a365ea69f..d8de8a61f9 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -211,6 +211,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // Check no movement because of invalid commands fixture.Server()->Run(true, 100, false); + ASSERT_FALSE(modelPoses.empty()); EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); @@ -244,10 +245,9 @@ void ThrusterTest::TestWorld(const std::string &_world, // Check movement if (_namespace != "lowbattery") { - for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; - ++sleep) + for (sleep = 0; (modelPoses.empty() || modelPoses.back().Pos().X() < 5.0) && + sleep < maxSleep; ++sleep) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); fixture.Server()->Run(true, 100, false); } EXPECT_LT(sleep, maxSleep); @@ -331,13 +331,11 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); } - modelPoses.clear(); propellerAngVels.clear(); propellerLinVels.clear(); // Make sure that when the deadband is disabled // commands below the deadband should create a movement - auto latest_pose = modelPoses.back(); msgs::Boolean db_msg; if (_namespace == "deadband") { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 58ab697e9a..e827452f29 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -721,6 +721,16 @@ TEST_F(TriggeredPublisherTest, std::string service = "/srv-test"; node.Advertise(service, srvEchoCb); + { + // This block of code is here because service requests from a previous test + // might interfere with this test. We sleep a small amount time and reset + // `recvCount` to 0. This ensures that the service requets from the previous + // test are discarded properly. + // TODO(azeey) Remove once + // https://github.com/gazebosim/gz-transport/issues/491 is resolved. + GZ_SLEEP_MS(2000); + recvCount = 0; + } const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 08453c8933..fef0017caf 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -526,23 +526,14 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) testSlipSystem.OnPreUpdate([&](const UpdateInfo &, EntityComponentManager &) { - auto wheelRearLeftVelocity0Cmd = - ecm->Component(wheelRearLeftSpin0Entity); - auto wheelRearRightVelocity0Cmd = - ecm->Component(wheelRearRightSpin0Entity); - auto wheelRearLeftVelocity1Cmd = - ecm->Component(wheelRearLeftSpin1Entity); - auto wheelRearRightVelocity1Cmd = - ecm->Component(wheelRearRightSpin1Entity); - - *wheelRearLeftVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity0Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearLeftVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); - *wheelRearRightVelocity1Cmd = - components::JointVelocityCmd({angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearLeftSpin1Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin0Entity, {angularSpeed}); + ecm->SetComponentData( + wheelRearRightSpin1Entity, {angularSpeed}); }); server.AddSystem(testSlipSystem.systemPtr); server.Run(true, 2000, false); diff --git a/test/worlds/README.md b/test/worlds/README.md new file mode 100644 index 0000000000..faaa619e1d --- /dev/null +++ b/test/worlds/README.md @@ -0,0 +1,9 @@ +# Integration Testing Files + +## Purpose + +This folder contains world files specifically designed for purpose of integration testing of system plugins. **They are strictly for testing and not meant to be run as examples.** + +## Running system plugin examples + +The world files are present in the `examples/worlds` directory of this repository. diff --git a/test/worlds/lighter_than_air_dynamics.sdf b/test/worlds/lighter_than_air_dynamics.sdf new file mode 100644 index 0000000000..223d8b7bcd --- /dev/null +++ b/test/worlds/lighter_than_air_dynamics.sdf @@ -0,0 +1,136 @@ + + + + + + 0.001 + + 0 + + + 0 0 0 + + + + + + 0 0 30 0 0 0 + + 0 0 0 0 0 0 + + 5.815677050417254 + + 0.0069861272 + 0 + 0 + 0.1292433532 + 0 + 0.1292433532 + + + + 0.0256621271421697 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + 0.538639694339695 + 0.0 + 0.0 + 0.0 + 0.189291512412098 + + 0.538639694339695 + 0.0 + -0.189291512412098 + 0.0 + + 0.000160094457776136 + 0.0 + 0.0 + + 2.02957381640185 + 0.0 + + 2.02957381640185 + + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + + + hull_link + 1.2754 + -0.06425096870274437 + 0.07237374316691345 + 0.08506449448628849 + -0.0941254292661463 + 0.1 + 2.4389047 + + + + + 0 5 30 0 0 0 + + 0 0 0 0 0 0 + + 5.815677050417254 + + 0.0069861272 + 0 + 0 + 0.1292433532 + 0 + 0.1292433532 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + + + + diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf index bc93489ee5..b45f431aa4 100644 --- a/test/worlds/quadcopter.sdf +++ b/test/worlds/quadcopter.sdf @@ -91,7 +91,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -136,7 +136,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -181,7 +181,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -226,7 +226,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf index 09390c0274..1650b5852c 100644 --- a/test/worlds/quadcopter_velocity_control.sdf +++ b/test/worlds/quadcopter_velocity_control.sdf @@ -91,7 +91,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -136,7 +136,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -181,7 +181,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2 @@ -226,7 +226,7 @@ - 0 0 0 1.57 0 0 0 + 0 0 0 1.57 0 0 0.2