diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 77df164d6e..9799421f24 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-math7 python3-gz-msgs10 python3-gz-transport13 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 f131a19a66..a85cfe43ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sim8 VERSION 8.2.0) +project(gz-sim8 VERSION 8.5.0) set (GZ_DISTRIBUTION "Harmonic") #============================================================================ @@ -66,7 +66,7 @@ cmake_dependent_option(USE_DIST_PACKAGES_FOR_PYTHON #============================================================================ # Setting this policy enables using the protobuf_MODULE_COMPATIBLE -# set command in CMake versions older than 13.13 +# set command when cmake_minimum_required is less than 3.13 set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # This option is needed to use the PROTOBUF_GENERATE_CPP # in case protobuf is found with the CMake config files @@ -136,7 +136,7 @@ set(GZ_PHYSICS_VER ${gz-physics7_VERSION_MAJOR}) #-------------------------------------- # Find gz-sensors -gz_find_package(gz-sensors8 REQUIRED +gz_find_package(gz-sensors8 REQUIRED VERSION 8.2.0 # component order is important COMPONENTS # non-rendering @@ -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 0a1349e075..7a4ba62198 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,134 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.5.0 (2024-06-26) + +1. Backport: Adding cone primitives + * [Pull request #2404](https://github.com/gazebosim/gz-sim/pull/2404) + +1. Permit to run gz sim -g on Windows + * [Pull request #2382](https://github.com/gazebosim/gz-sim/pull/2382) + +1. Parse voxel resolution SDF param when decomposing meshes + * [Pull request #2445](https://github.com/gazebosim/gz-sim/pull/2445) + +1. Fix model command api test + * [Pull request #2444](https://github.com/gazebosim/gz-sim/pull/2444) + +1. Add tutorial for using the Pose component + * [Pull request #2219](https://github.com/gazebosim/gz-sim/pull/2219) + +1. Do not update sensors if it a triggered sensor + * [Pull request #2443](https://github.com/gazebosim/gz-sim/pull/2443) + +### Gazebo Sim 8.4.0 (2024-06-12) + +1. Add pause run tutorial + * [Pull request #2383](https://github.com/gazebosim/gz-sim/pull/2383) + +1. Fix warning message to show precise jump back in time duration + * [Pull request #2435](https://github.com/gazebosim/gz-sim/pull/2435) + +1. Optimize rendering sensor pose updates + * [Pull request #2425](https://github.com/gazebosim/gz-sim/pull/2425) + +1. Remove a few extra zeros from some sdf files + * [Pull request #2426](https://github.com/gazebosim/gz-sim/pull/2426) + +1. Use VERSION_GREATER_EQUAL in cmake logic + * [Pull request #2418](https://github.com/gazebosim/gz-sim/pull/2418) + +1. Support mesh optimization when using AttachMeshShapeFeature + * [Pull request #2417](https://github.com/gazebosim/gz-sim/pull/2417) + +1. Rephrase cmake comment about CMP0077 + * [Pull request #2419](https://github.com/gazebosim/gz-sim/pull/2419) + +1. Fix CMake warnings in Noble + * [Pull request #2397](https://github.com/gazebosim/gz-sim/pull/2397) + +1. Update sensors with pending trigger immediately in Sensors system + * [Pull request #2408](https://github.com/gazebosim/gz-sim/pull/2408) + +1. Add missing algorithm include + * [Pull request #2414](https://github.com/gazebosim/gz-sim/pull/2414) + +1. Add Track and Follow options in gui EntityContextMenu + * [Pull request #2402](https://github.com/gazebosim/gz-sim/pull/2402) + +1. ForceTorque system: improve readability + * [Pull request #2403](https://github.com/gazebosim/gz-sim/pull/2403) + +1. LTA Dynamics System + * [Pull request #2241](https://github.com/gazebosim/gz-sim/pull/2241) + +1. Remove Empty Test File + * [Pull request #2396](https://github.com/gazebosim/gz-sim/pull/2396) + +1. Fix GCC/CMake warnings for Noble + * [Pull request #2375](https://github.com/gazebosim/gz-sim/pull/2375) + +1. Fix warn unused variable in test + * [Pull request #2388](https://github.com/gazebosim/gz-sim/pull/2388) + +1. Fix name of gz-fuel_tools in package.xml + * [Pull request #2386](https://github.com/gazebosim/gz-sim/pull/2386) + +1. Add package.xml + * [Pull request #2337](https://github.com/gazebosim/gz-sim/pull/2337) + +1. Fix namespace and class links in documentation references that use namespace `gz` + * [Pull request #2385](https://github.com/gazebosim/gz-sim/pull/2385) + +1. Fix ModelPhotoShootTest test failures + * [Pull request #2294](https://github.com/gazebosim/gz-sim/pull/2294) + +1. Enable StoreResolvedURIs when loading SDF + * [Pull request #2349](https://github.com/gazebosim/gz-sim/pull/2349) + +1. Drop python3-disttutils from apt packages files + * [Pull request #2374](https://github.com/gazebosim/gz-sim/pull/2374) + +1. Added example world for `DopplerVelocityLogSystem` + * [Pull request #2373](https://github.com/gazebosim/gz-sim/pull/2373) + +1. Fix Gazebo/White and refactored MaterialParser + * [Pull request #2302](https://github.com/gazebosim/gz-sim/pull/2302) + +1. Support for Gazebo materials + * [Pull request #2269](https://github.com/gazebosim/gz-sim/pull/2269) + +### 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. diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 32bb92dac3..09abde0f62 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 376497eeaf..5c127f7f45 100644 --- a/examples/standalone/gtest_setup/CMakeLists.txt +++ b/examples/standalone/gtest_setup/CMakeLists.txt @@ -10,13 +10,14 @@ set(GZ_SIM_VER ${gz-sim8_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/examples/worlds/shapes.sdf b/examples/worlds/shapes.sdf index 1d75661b49..5d020a8c32 100644 --- a/examples/worlds/shapes.sdf +++ b/examples/worlds/shapes.sdf @@ -1,12 +1,13 @@ - - + + + 1.0 1.0 1.0 @@ -240,5 +241,36 @@ Try moving a model: + + + 0 4.5 0.5 0 0 0 + + + 1 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 1 0.47 0 1 + 1 0.47 0 1 + 1 0.47 0 1 + + + + 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/Primitives.hh b/include/gz/sim/Primitives.hh index 84bc8344f5..b90b9e7553 100644 --- a/include/gz/sim/Primitives.hh +++ b/include/gz/sim/Primitives.hh @@ -35,6 +35,7 @@ namespace gz { kBox, kCapsule, + kCone, kCylinder, kEllipsoid, kSphere, @@ -67,8 +68,8 @@ namespace gz /// \brief Return an SDF string of one of the available primitive shape or /// light types. /// \param[in] _typeName Type name of the of shape or light to retrieve. - /// Must be one of: box, sphere, cylinder, capsule, ellipsoid, directional, - /// point, or spot. + /// Must be one of: box, sphere, cylinder, cone, capsule, ellipsoid, + /// directional, point, or spot. /// \return String containing SDF description of primitive shape or light. /// Empty string if the _typeName is invalid. std::string GZ_SIM_VISIBLE 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/Util.hh b/include/gz/sim/Util.hh index e8e4bed0e0..de0585e5c0 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -316,6 +316,13 @@ namespace gz /// \return The loaded mesh or null if the mesh can not be loaded. GZ_SIM_VISIBLE const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf); + /// \brief Optimize input mesh. + /// \param[in] _meshSdf Mesh SDF DOM with mesh optimization parameters + /// \param[in] _mesh Input mesh to optimize. + /// \return The optimized mesh or null if the mesh can not be optimized. + GZ_SIM_VISIBLE const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, + const common::Mesh &_mesh); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; 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/include/gz/sim/components/Gravity.hh b/include/gz/sim/components/Gravity.hh index 82ba3d036a..a4b8ac352d 100644 --- a/include/gz/sim/components/Gravity.hh +++ b/include/gz/sim/components/Gravity.hh @@ -36,6 +36,11 @@ namespace components /// \brief Store the gravity acceleration. using Gravity = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Gravity", Gravity) + + /// \brief Store the gravity acceleration. + using GravityEnabled = Component; + GZ_SIM_REGISTER_COMPONENT( + "gz_sim_components.GravityEnabled", GravityEnabled) } } } diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000..a655c2508c --- /dev/null +++ b/package.xml @@ -0,0 +1,55 @@ + + + + gz-sim8 + 8.5.0 + Gazebo Sim : A Robotic Simulator + Michael Carroll + Apache License 2.0 + https://github.com/gazebosim/gz-sim + + cmake + + benchmark + glut + gz-cmake3 + gz-common5 + gz-fuel_tools9 + gz-gui8 + gz-math7 + gz-msgs10 + gz-physics7 + gz-plugin2 + gz-rendering8 + gz-sensors8 + gz-tools2 + gz-transport13 + gz-utils2 + 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 + sdformat14 + tinyxml2 + uuid + + xvfb + python3-pytest + + + cmake + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 667562dc45..b3c2917657 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 @@ -314,7 +309,7 @@ foreach(CMD_TEST # to the PATH. This is done via the ENVIRONMENT_MODIFICATION that is only available # since CMake 3.22. However, if an older CMake is used another trick to install the libraries # beforehand - if (WIN32 AND CMAKE_VERSION STRGREATER "3.22") + if (WIN32 AND ${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.22") set_tests_properties(${CMD_TEST} PROPERTIES ENVIRONMENT_MODIFICATION "PATH=path_list_prepend:${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") endif() diff --git a/src/Conversions.cc b/src/Conversions.cc index fbe7587141..e6c94f7591 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -176,6 +178,12 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) out.mutable_capsule()->set_radius(_in.CapsuleShape()->Radius()); out.mutable_capsule()->set_length(_in.CapsuleShape()->Length()); } + else if (_in.Type() == sdf::GeometryType::CONE && _in.ConeShape()) + { + out.set_type(msgs::Geometry::CONE); + out.mutable_cone()->set_radius(_in.ConeShape()->Radius()); + out.mutable_cone()->set_length(_in.ConeShape()->Length()); + } else if (_in.Type() == sdf::GeometryType::CYLINDER && _in.CylinderShape()) { out.set_type(msgs::Geometry::CYLINDER); @@ -260,6 +268,10 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) } } } + else if (_in.Type() == sdf::GeometryType::EMPTY) + { + out.set_type(msgs::Geometry::EMPTY); + } else { gzerr << "Geometry type [" << static_cast(_in.Type()) @@ -293,6 +305,16 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in) out.SetCapsuleShape(capsuleShape); } + else if (_in.type() == msgs::Geometry::CONE && _in.has_cone()) + { + out.SetType(sdf::GeometryType::CONE); + + sdf::Cone coneShape; + coneShape.SetRadius(_in.cone().radius()); + coneShape.SetLength(_in.cone().length()); + + out.SetConeShape(coneShape); + } else if (_in.type() == msgs::Geometry::CYLINDER && _in.has_cylinder()) { out.SetType(sdf::GeometryType::CYLINDER); diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 08ac3e1afd..2d67bcfe29 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1203,3 +1203,13 @@ TEST(Conversions, MsgsPluginToSdf) EXPECT_EQ(innerXml, sdfPlugins[1].Contents()[0]->ToString("")); EXPECT_EQ(innerXml2, sdfPlugins[1].Contents()[1]->ToString("")); } + +///////////////////////////////////////////////// +TEST(Conversions, GeometryEmpty) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::EMPTY); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::EMPTY, geometryMsg.type()); +} 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/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 95f5b8b4a4..36e088680a 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -608,7 +608,7 @@ TEST(ModelCommandAPI, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) // Run without blocking. server.Run(false, 0, false); - // Tested command: gz model -m altimeter_mode -l link -s altimeter_sensor + // Tested command: gz model -m rgbd_camera -l rgbd_camera_link -s rgbd_camera { const std::string cmd = kGzModelCommand + "-m rgbd_camera -l rgbd_camera_link -s rgbd_camera"; @@ -657,7 +657,7 @@ TEST(ModelCommandAPI, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) " - Lens intrinsics Fy: 277\n" " - Lens intrinsics Cx: 160\n" " - Lens intrinsics Cy: 120\n" - " - Lens intrinsics skew: 1\n" + " - Lens intrinsics skew: 0\n" " - Visibility mask: 4294967295\n"; EXPECT_EQ(expectedOutput, output); } diff --git a/src/Primitives.cc b/src/Primitives.cc index 4fa93cea54..fcb7f65942 100644 --- a/src/Primitives.cc +++ b/src/Primitives.cc @@ -104,6 +104,49 @@ constexpr const char * kSphereSdf = R"( )"; +///////////////////////////////////////////////// +constexpr const char * kConeSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.075 + 0 + 0 + 0.075 + 0 + 0.075 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + 0.5 + 1.0 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + ///////////////////////////////////////////////// constexpr const char * kCylinderSdf = R"( @@ -301,6 +344,8 @@ std::string gz::sim::getPrimitiveShape(const PrimitiveShape &_type) return kBoxSdf; case PrimitiveShape::kSphere: return kSphereSdf; + case PrimitiveShape::kCone: + return kConeSdf; case PrimitiveShape::kCylinder: return kCylinderSdf; case PrimitiveShape::kCapsule: @@ -339,6 +384,8 @@ std::string gz::sim::getPrimitive(const std::string &_typeName) return getPrimitiveShape(PrimitiveShape::kSphere); else if (type == "cylinder") return getPrimitiveShape(PrimitiveShape::kCylinder); + else if (type == "cone") + return getPrimitiveShape(PrimitiveShape::kCone); else if (type == "capsule") return getPrimitiveShape(PrimitiveShape::kCapsule); else if (type == "ellipsoid") @@ -354,6 +401,7 @@ std::string gz::sim::getPrimitive(const std::string &_typeName) gzwarn << "The valid options are:\n"; gzwarn << " - box\n"; gzwarn << " - sphere\n"; + gzwarn << " - cone\n"; gzwarn << " - cylinder\n"; gzwarn << " - capsule\n"; gzwarn << " - ellipsoid\n"; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index f4b693533b..082f42db11 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -572,6 +572,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) linkEntity, components::WindMode(_link->EnableWind())); } + if (!_link->EnableGravity()) + { + // If disable gravity, create a GravityEnabled component to the entity + this->dataPtr->ecm->CreateComponent( + linkEntity, components::GravityEnabled(false)); + } + // Visuals for (uint64_t visualIndex = 0; visualIndex < _link->VisualCount(); ++visualIndex) 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/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..c73ac54dbc 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -15,12 +15,17 @@ * */ +#include +#include +#include + #include #include #include #include #include +#include #include #include #include @@ -397,12 +402,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 @@ -855,8 +867,85 @@ const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf) << "]." << std::endl; return nullptr; } + + if (mesh && _meshSdf.Optimization() != sdf::MeshOptimization::NONE) + { + const common::Mesh *optimizedMesh = optimizeMesh(_meshSdf, *mesh); + if (optimizedMesh) + return optimizedMesh; + else + gzwarn << "Failed to optimize Mesh " << mesh->Name() << std::endl; + } + return mesh; } + +const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf, + const common::Mesh &_mesh) +{ + if (_meshSdf.Optimization() != + sdf::MeshOptimization::CONVEX_DECOMPOSITION && + _meshSdf.Optimization() != + sdf::MeshOptimization::CONVEX_HULL) + return nullptr; + + auto &meshManager = *common::MeshManager::Instance(); + std::size_t maxConvexHulls = 16u; + std::size_t voxelResolution = 200000u; + if (_meshSdf.ConvexDecomposition()) + { + // limit max number of convex hulls to generate + maxConvexHulls = _meshSdf.ConvexDecomposition()->MaxConvexHulls(); + voxelResolution = _meshSdf.ConvexDecomposition()->VoxelResolution(); + } + if (_meshSdf.Optimization() == sdf::MeshOptimization::CONVEX_HULL) + { + /// create 1 convex hull for the whole submesh + maxConvexHulls = 1u; + } + + // Check if MeshManager contains the decomposed mesh already. If not + // add it to the MeshManager so we do not need to decompose it again. + const std::string convexMeshName = + _mesh.Name() + "_" + _meshSdf.Submesh() + "_CONVEX_" + + std::to_string(maxConvexHulls) + "_" + std::to_string(voxelResolution); + auto *optimizedMesh = meshManager.MeshByName(convexMeshName); + if (!optimizedMesh) + { + // Merge meshes before convex decomposition + auto mergedMesh = gz::common::MeshManager::MergeSubMeshes(_mesh); + if (mergedMesh && mergedMesh->SubMeshCount() == 1u) + { + // Decompose and add mesh to MeshManager + auto mergedSubmesh = mergedMesh->SubMeshByIndex(0u).lock(); + std::vector decomposed = + gz::common::MeshManager::ConvexDecomposition( + *mergedSubmesh.get(), maxConvexHulls, voxelResolution); + gzdbg << "Optimizing mesh (" << _meshSdf.OptimizationStr() << "): " + << _mesh.Name() << std::endl; + // Create decomposed mesh and add it to MeshManager + // Note: MeshManager will call delete on this mesh in its destructor + // \todo(iche033) Consider updating MeshManager to accept + // unique pointers instead + common::Mesh *convexMesh = new common::Mesh; + convexMesh->SetName(convexMeshName); + for (const auto & submesh : decomposed) + convexMesh->AddSubMesh(submesh); + meshManager.AddMesh(convexMesh); + if (decomposed.empty()) + { + // Print an error if convex decomposition returned empty submeshes + // but still add it to MeshManager to avoid going through the + // expensive convex decomposition process for the same mesh again + gzerr << "Convex decomposition generated zero meshes: " + << _mesh.Name() << std::endl; + } + optimizedMesh = meshManager.MeshByName(convexMeshName); + } + } + return optimizedMesh; +} + } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 6be8d27903..9ec0e9be1a 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -1023,4 +1023,12 @@ TEST_F(UtilTest, LoadMesh) "test", "media", "duck.dae"); meshSdf.SetFilePath(filePath); EXPECT_NE(nullptr, loadMesh(meshSdf)); + + EXPECT_TRUE(meshSdf.SetOptimization("convex_decomposition")); + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(16u); + meshSdf.SetConvexDecomposition(convexDecomp); + auto *optimizedMesh = loadMesh(meshSdf); + EXPECT_NE(nullptr, optimizedMesh); + EXPECT_EQ(16u, optimizedMesh->SubMeshCount()); } 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/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index 64ee638c7b..65946d3494 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -598,12 +598,6 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." options['record-period'], options['seed']) # Otherwise run the gui else options['gui'] - if plugin.end_with? ".dll" - puts "`gz sim` currently only works with the -s argument on Windows. -See https://github.com/gazebosim/gz-sim/issues/168 for more info." - exit(-1) - end - ENV['RMT_PORT'] = '1501' Importer.runGui(options['gui_config'], options['file'], options['wait_gui'], options['render_engine_gui'], 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/component_inspector_editor/ComponentInspectorEditor.qml b/src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml index f1e47ccd81..c21f8787f1 100644 --- a/src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml +++ b/src/gui/plugins/component_inspector_editor/ComponentInspectorEditor.qml @@ -373,6 +373,14 @@ Rectangle { } } + MenuItem { + id: coneLink + text: "Cone" + onClicked: { + ComponentInspectorEditor.OnAddEntity("cone", "link"); + } + } + MenuItem { id: cylinderLink text: "Cylinder" diff --git a/src/gui/plugins/component_inspector_editor/ModelEditor.cc b/src/gui/plugins/component_inspector_editor/ModelEditor.cc index a838054b80..2f32916f66 100644 --- a/src/gui/plugins/component_inspector_editor/ModelEditor.cc +++ b/src/gui/plugins/component_inspector_editor/ModelEditor.cc @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -330,6 +331,14 @@ std::optional ModelEditorPrivate::CreateGeom( geom.SetSphereShape(shape); geom.SetType(sdf::GeometryType::SPHERE); } + else if (_eta.geomOrLightType == "cone") + { + sdf::Cone shape; + shape.SetRadius(size.X() * 0.5); + shape.SetLength(size.Z()); + geom.SetConeShape(shape); + geom.SetType(sdf::GeometryType::CONE); + } else if (_eta.geomOrLightType == "cylinder") { sdf::Cylinder shape; diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml index 8b6db13123..1d65b9e7be 100644 --- a/src/gui/plugins/entity_tree/EntityTree.qml +++ b/src/gui/plugins/entity_tree/EntityTree.qml @@ -202,6 +202,15 @@ Rectangle { } } + MenuItem + { + id: cone + text: "Cone" + onClicked: { + EntityTree.OnInsertEntity("cone") + } + } + MenuItem { id: cylinder 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/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml index 781eaca28f..fb80c1431f 100644 --- a/src/gui/plugins/shapes/Shapes.qml +++ b/src/gui/plugins/shapes/Shapes.qml @@ -67,6 +67,23 @@ ToolBar { Shapes.OnMode("sphere") } } + ToolButton { + id: cone + ToolTip.text: "Cone" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "cone.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + Shapes.OnMode("cone") + } + } ToolButton { id: cylinder ToolTip.text: "Cylinder" diff --git a/src/gui/plugins/shapes/Shapes.qrc b/src/gui/plugins/shapes/Shapes.qrc index 754d997c5d..00fddab57c 100644 --- a/src/gui/plugins/shapes/Shapes.qrc +++ b/src/gui/plugins/shapes/Shapes.qrc @@ -3,6 +3,7 @@ Shapes.qml box.png sphere.png + cone.png cylinder.png capsule.png ellipsoid.png diff --git a/src/gui/plugins/shapes/cone.png b/src/gui/plugins/shapes/cone.png new file mode 100644 index 0000000000..53f26d831c Binary files /dev/null and b/src/gui/plugins/shapes/cone.png differ diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index c5d24bc7ab..2694553d54 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -63,6 +63,7 @@ #include #include +#include #include #include #include @@ -1191,6 +1192,13 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( capsule->SetLength(_geom.CapsuleShape()->Length()); geom = capsule; } + else if (_geom.Type() == sdf::GeometryType::CONE) + { + geom = this->scene->CreateCone(); + scale.X() = _geom.ConeShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _geom.ConeShape()->Length(); + } else if (_geom.Type() == sdf::GeometryType::CYLINDER) { geom = this->scene->CreateCylinder(); 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/MarkerManager.cc b/src/rendering/MarkerManager.cc index 50b761cecb..04b1bef0e1 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -387,6 +387,8 @@ rendering::MarkerType MarkerManagerPrivate::MsgToType( return rendering::MarkerType::MT_BOX; case msgs::Marker::CAPSULE: return rendering::MarkerType::MT_CAPSULE; + case msgs::Marker::CONE: + return rendering::MarkerType::MT_CONE; case msgs::Marker::CYLINDER: return rendering::MarkerType::MT_CYLINDER; case msgs::Marker::LINE_STRIP: diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 0c833a60ca..9b2cbf8acd 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -1323,6 +1324,18 @@ void RenderUtil::Update() gzerr << "Failed to create sensor [" << sensorName << "]" << std::endl; } + else + { + auto sensorNode = this->dataPtr->sceneManager.NodeById(entity); + auto semPose = dataSdf.SemanticPose(); + math::Pose3d sensorPose; + sdf::Errors errors = semPose.Resolve(sensorPose); + if (!errors.empty()) + { + sensorPose = dataSdf.RawPose(); + } + sensorNode->SetLocalPose(sensorPose); + } } } } @@ -2388,86 +2401,6 @@ void RenderUtilPrivate::UpdateRenderingEntities( this->entityPoses[_entity] = _pose->Data(); return true; }); - - // Update cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update RGBD cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update gpu_lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update thermal cameras - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update segmentation cameras - _ecm.Each( - [&](const Entity &_entity, - const components::SegmentationCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update bounding box cameras - _ecm.Each( - [&](const Entity &_entity, - const components::BoundingBoxCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); - - // Update wide angle cameras - _ecm.Each( - [&](const Entity &_entity, - const components::WideAngleCamera *, - const components::Pose *_pose)->bool - { - this->entityPoses[_entity] = _pose->Data(); - return true; - }); } ////////////////////////////////////////////////// diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index d94e6aa8b7..c1aefe8918 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -667,6 +668,13 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, capsule->SetLength(_geom.CapsuleShape()->Length()); geom = capsule; } + else if (_geom.Type() == sdf::GeometryType::CONE) + { + geom = this->dataPtr->scene->CreateCone(); + scale.X() = _geom.ConeShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _geom.ConeShape()->Length(); + } else if (_geom.Type() == sdf::GeometryType::CYLINDER) { geom = this->dataPtr->scene->CreateCylinder(); 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 0d468916e5..59d81052f8 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -508,8 +508,8 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them @@ -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; } ////////////////////////////////////////////////// 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/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc index a7c60109fd..80065691d5 100644 --- a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc @@ -802,8 +802,8 @@ void AdvancedLiftDrag::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!this->dataPtr->initialized) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 7dba18337f..3a99824e76 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -132,8 +132,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc index 73c9e92466..39904b9575 100644 --- a/src/systems/air_speed/AirSpeed.cc +++ b/src/systems/air_speed/AirSpeed.cc @@ -134,8 +134,8 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index ecd3ee1ea1..7f45d2899f 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -132,8 +132,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index 19add069df..b7ea5eba18 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -134,8 +134,8 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joint hasn't been identified yet, look for it diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 357b01df97..523950b172 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -550,8 +550,8 @@ void LinearBatteryPlugin::Update(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (_info.paused) diff --git a/src/systems/contact/Contact.cc b/src/systems/contact/Contact.cc index f4b8019f0c..0dcea6a8b4 100644 --- a/src/systems/contact/Contact.cc +++ b/src/systems/contact/Contact.cc @@ -276,8 +276,8 @@ void Contact::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!_info.paused) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 700e342743..b65af3fce6 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -396,8 +396,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them @@ -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..8792f514d5 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -166,8 +166,8 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); @@ -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/imu/Imu.cc b/src/systems/imu/Imu.cc index eebbe77b6c..6026cda124 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -136,8 +136,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 4c18ac7bdf..cf8f54187c 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -265,8 +265,8 @@ void JointController::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them @@ -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..4b36a5076c 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -339,8 +339,8 @@ void JointPositionController::PreUpdate( if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them @@ -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/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index c83837c5ff..902bfec942 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -550,8 +550,8 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } if (!this->dataPtr->initialized) 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/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 8cbeadbfb1..3361c407f4 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -669,8 +669,8 @@ void LogRecord::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Publish only once diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index a4e5a7f195..a460e620c2 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -405,12 +405,11 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( }; // create services for this source - const auto fullName = scopedName(entity, _ecm); - auto validName = transport::TopicUtils::AsValidTopic(fullName); + const auto validName = topicFromScopedName(entity, _ecm, false); if (validName.empty()) { gzerr << "Failed to create valid topics with entity scoped name [" - << fullName << "]" << std::endl; + << scopedName(entity, _ecm) << "]" << std::endl; return; } if (!this->node.Advertise(validName + "/play", playSrvCb)) @@ -504,7 +503,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( // create the detection publisher for this microphone auto pub = this->node.Advertise( - scopedName(entity, _ecm) + "/detection"); + topicFromScopedName(entity, _ecm, false) + "/detection"); if (!pub) { gzerr << "Error creating a detection publisher for microphone " diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 1ddfe2146f..ded8c9c36f 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -134,8 +134,8 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 499664a046..82b4e8d0f4 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -274,8 +274,8 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 6d77de4c77..732bc07b1d 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -333,8 +333,8 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joints haven't been identified yet, look for them @@ -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_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index 96f547fa4d..d352a68a7a 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -349,8 +349,8 @@ void MulticopterVelocityControl::PreUpdate( if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 8e6e421ce9..2bfe779776 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -399,8 +399,8 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // If the joint or links haven't been identified yet, look for them diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index f438a9d240..dd6ac144f7 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -128,8 +128,8 @@ void NavSat::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } this->dataPtr->CreateSensors(_ecm); diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index dc96b6b51b..66e42b1a08 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -59,7 +59,9 @@ class gz::sim::systems::OdometryPublisherPrivate public: transport::Node node; /// \brief Model interface + //! [modelDeclaration] public: Model model{kNullEntity}; + //! [modelDeclaration] /// \brief Name of the world-fixed coordinate frame for the odometry message. public: std::string odomFrame; @@ -123,12 +125,14 @@ OdometryPublisher::OdometryPublisher() } ////////////////////////////////////////////////// +//! [Configure] void OdometryPublisher::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) { this->dataPtr->model = Model(_entity); + //! [Configure] if (!this->dataPtr->model.Valid(_ecm)) { @@ -223,8 +227,10 @@ void OdometryPublisher::Configure(const Entity &_entity, } else { + //! [definePub] this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); + //! [definePub] gzmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid << "]" << std::endl; } @@ -287,17 +293,8 @@ void OdometryPublisher::PreUpdate(const gz::sim::UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - // Create the pose component if it does not exist. - auto pos = _ecm.Component( - this->dataPtr->model.Entity()); - if (!pos) - { - _ecm.CreateComponent(this->dataPtr->model.Entity(), - components::Pose()); + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } } @@ -337,7 +334,9 @@ void OdometryPublisherPrivate::UpdateOdometry( } // Construct the odometry message and publish it. + //! [declarePoseMsg] msgs::Odometry msg; + //! [declarePoseMsg] const std::chrono::duration dt = std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; @@ -347,7 +346,10 @@ void OdometryPublisherPrivate::UpdateOdometry( return; // Get and set robotBaseFrame to odom transformation. + //! [worldPose] const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); + //! [worldPose] + //! [setPoseMsg] math::Pose3d pose = rawPose * this->offset; msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); @@ -356,6 +358,7 @@ void OdometryPublisherPrivate::UpdateOdometry( { msg.mutable_pose()->mutable_position()->set_z(pose.Pos().Z()); } + //! [setPoseMsg] // Get linear and angular displacements from last updated pose. double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); @@ -455,7 +458,9 @@ void OdometryPublisherPrivate::UpdateOdometry( this->lastOdomPubTime = _info.simTime; if (this->odomPub.Valid()) { + //! [publishMsg] this->odomPub.Publish(msg); + //! [publishMsg] } // Generate odometry with covariance message and publish it. diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index cab9bd6d14..33c8fd23af 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -76,6 +76,13 @@ namespace systems /// - ``: Standard deviation of the Gaussian noise to be added /// to pose and twist messages. This element is optional, and the default /// value is 0. + /// + /// ## Components + /// + /// This system uses the following components: + /// + /// - gz::sim::components::Pose: Pose represented by gz::math::Pose3d. Used + /// to calculate the odometry to publish. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1f6defbf44..93a41a3f80 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -53,6 +53,7 @@ #include #include +#include #include #include #include @@ -621,6 +622,14 @@ class gz::sim::systems::PhysicsPrivate public: struct SolverFeatureList : gz::physics::FeatureList< gz::physics::Solver>{}; + ////////////////////////////////////////////////// + // CollisionPairMaxContacts + /// \brief Feature list for setting and getting the max total contacts for + /// collision pairs + public: struct CollisionPairMaxContactsFeatureList : + gz::physics::FeatureList< + gz::physics::CollisionPairMaxContacts>{}; + ////////////////////////////////////////////////// // Nested Models @@ -646,7 +655,8 @@ class gz::sim::systems::PhysicsPrivate NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList, - WorldModelFeatureList + WorldModelFeatureList, + CollisionPairMaxContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1025,6 +1035,33 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + auto physicsComp = + _ecm.Component(_entity); + if (physicsComp) + { + auto maxContactsFeature = + this->entityWorldMap.EntityCast< + CollisionPairMaxContactsFeatureList>(_entity); + if (!maxContactsFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[CollisionPairMaxContacts]. " + << "Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + maxContactsFeature->SetCollisionPairMaxContacts( + physicsComp->Data().MaxContacts()); + } + } + // World Model proxy (used for joints directly under in SDF) auto worldModelFeature = this->entityWorldMap.EntityCast(_entity); @@ -1286,6 +1323,15 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm, link.SetInertial(inertial->Data()); } + // get link gravity + const components::GravityEnabled *gravityEnabled = + _ecm.Component(_entity); + if (nullptr != gravityEnabled) + { + // gravityEnabled set in SdfEntityCreator::CreateEntities() + link.SetEnableGravity(gravityEnabled->Data()); + } + auto constructLinkFeature = this->entityModelMap.EntityCast( _parent->Data()); diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index cec62d341c..256ee5ac75 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -26,6 +26,7 @@ // Features need to be defined ahead of entityCast #include #include +#include #include #include #include diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 58661d4816..28fbbdc8d4 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -252,8 +252,7 @@ void PosePublisher::Configure(const Entity &_entity, this->dataPtr->usePoseV = _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; - std::string poseTopic = scopedName(_entity, _ecm) + "/pose"; - poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + std::string poseTopic = topicFromScopedName(_entity, _ecm, false) + "/pose"; if (poseTopic.empty()) { poseTopic = "/pose"; @@ -297,8 +296,8 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 3ce3f4b619..601a4b3c9d 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( @@ -1024,6 +1034,11 @@ std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( continue; } + if (rs->IsTriggered()) + { + continue; + } + std::chrono::steady_clock::duration time; // if sensor's next update tims is less or equal to current sim time then // it's in the process of being updated by the render loop @@ -1075,6 +1090,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/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index 91007cfbcc..d9cab9fd1d 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -251,8 +251,8 @@ void TouchPluginPrivate::Update(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } { diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 68224b3bab..7af66e743b 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -188,8 +188,8 @@ void VelocityControl::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. @@ -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/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 5ef1e51aa4..e237d0b1bc 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -716,8 +716,8 @@ void WindEffects::PreUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Process commands 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 diff --git a/tutorials.md.in b/tutorials.md.in index 46c3f4be71..b75967e1ad 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -11,6 +11,7 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage gui_config "GUI configuration": Customizing your layout. * \subpage server_config "Server configuration": Customizing what system plugins are loaded. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. +* \subpage pause_run_simulation "Pause and Run simulation": Use Gazebo transport API to pause and run simulation. * \subpage reset_simulation Reset simulation * \subpage resources "Finding resources": The different ways in which Gazebo looks for files. * \subpage debugging "Debugging": Information about debugging Gazebo. diff --git a/tutorials/component_pose.md b/tutorials/component_pose.md new file mode 100644 index 0000000000..34d440e12e --- /dev/null +++ b/tutorials/component_pose.md @@ -0,0 +1,80 @@ +\page posecomponent Case Study: Using the Pose Component + +We will show how to use the gz::sim::components::Pose component in a system. + +An example usage of the component can be found in the +gz::sim::systems::OdometryPublisher system +([source code](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems/odometry_publisher)), +which reads the pose component of a model through the Model entity, uses the +pose for some calculations, and then publishes the result as a message. + +More usage can be found in the +[integration test](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration/odometry_publisher.cc) +for the system, with test worlds `odometry*.sdf` +[here](https://github.com/gazebosim/gz-sim/tree/main/test/worlds). + +### Objects of interest + +- gz::sim::components::Pose: A component containing pose information +- gz::math::Pose3d: The actual data underlying a pose component +- gz::sim::systems::OdometryPublisher: A system that reads the pose component + of a model +- gz::sim::Model: The type underlying a model entity (gz::sim::Entity) + +### Find the model entity + +First, we will need access to an entity, the \ref gz::sim::Model entity in this +case. +`OdometryPublisher` happens to be a system meant to be specified under `` +in the SDF, so at the time `Configure()` is called, it has access to a model +entity from which we can extract a \ref gz::sim::Model: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc modelDeclaration +\snippet src/systems/odometry_publisher/OdometryPublisher.cc Configure + +### Read the pose component + +Once we have the handle to an entity, we can access components associated with +it. +A component may have been created at the time the world is loaded, or you may +create a component at runtime if it does not exist yet. + +In this case, we use the model entity found above to access its pose component, +which is created by default on every model entity. + +In `PostUpdate()`, which happens after physics has updated, we can get the +world pose of a model through gz::sim::worldPose, by passing in the model +entity and the entity component manager. + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc worldPose + +It returns the raw data to us in the gz::math::Pose3d type, which is also the +data type underlying a pose component. +We can perform calculations on the gz::math::Pose3d type, not the +gz::sim::components::Pose type, which is just a wrapper. + +### Use the pose component + +Now we can use the pose data as we like. + +Here, we manipulate the pose and package the result into a gz::msgs::Odometry +message to be published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc declarePoseMsg + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc setPoseMsg + +See the source code for setting other fields of the message, such as twist and +the header. + +The message is then published: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc publishMsg + +where `odomPub` is defined in `Configure()`: + +\snippet src/systems/odometry_publisher/OdometryPublisher.cc definePub + +Outside the scope of this tutorial, the odometry publisher system also +calculates the covariance and publishes a pose vector on a TF topic. +See the source code to learn more. diff --git a/tutorials/files/pause_run_simlation/gui_pause_run.png b/tutorials/files/pause_run_simlation/gui_pause_run.png new file mode 100644 index 0000000000..49dd539653 Binary files /dev/null and b/tutorials/files/pause_run_simlation/gui_pause_run.png differ diff --git a/tutorials/pause_run_simulation.md b/tutorials/pause_run_simulation.md new file mode 100644 index 0000000000..ef757b75a0 --- /dev/null +++ b/tutorials/pause_run_simulation.md @@ -0,0 +1,57 @@ +\page pause_run_simulation Pause and Run simulation + +A Gazebo transport API is exposed to allow starting and stopping the simulation. +It's possible to call this API using the command line or through the GUI. + +To repeat this demo, run the `default` world: +```bash +gz sim default.sdf +``` + +## Transport API + +When Gazebo is run headless, this is an easy way to start the simulation. + +To pause and play over the transport API, we should call the service `/world//control` and fill the request message type +`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the request (true means everything was fine, false otherwise). + +The `WorldControl` message contains a `pause` field for starting and stopping the simulation. + +To start the simulation: + +```bash +gz service -s /world/default/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --timeout 3000 --req 'pause: false' +``` + +To pause the simulation: + +```bash +gz service -s /world/default/control --reqtype gz.msgs.WorldControl --reptype gz.msgs.Boolean --timeout 3000 --req 'pause: true' +``` + +When paused, time will stop in Gazebo, and the physics will not be running. + +To check the current state of the simulator, check the `paused` field in `/stats` like so: +```bash +gz topic --echo --topic /stats -n 1 +``` +If the simulator is currently paused but was running before, we would see something similar to this: +```text +sim_time { + sec: 8 + nsec: 707000000 +} +real_time { + sec: 8 + nsec: 824323281 +} +iterations: 8707 +real_time_factor: 0.998022916602211 +``` + + +## GUI + +We included a button in the `World Control` plugin allowing to start and stop the simulation from the GUI. + +@image html files/pause_run_simulation/gui_pause_run.png diff --git a/tutorials/reset_simulation.md b/tutorials/reset_simulation.md index 7a62a3fa3d..bf8ae121ab 100644 --- a/tutorials/reset_simulation.md +++ b/tutorials/reset_simulation.md @@ -4,6 +4,11 @@ The Reset Gazebo transport API is exposed to allow resetting simulation to time It's possible to call this API using the command line or through the GUI. In addition to the API, we have also expanded the simulation system API with a Reset interface. +To repeat this demo, run the `rolling_shapes.sdf` file: +```bash +gz sim rolling_shapes.sdf +``` + ## Reset interface System authors may now choose to implement the Reset interface to have a more intelligent @@ -16,7 +21,7 @@ Follow the tutorial \subpage createsystemplugins to see how to support Reset by ## Transport API To invoke reset over transport API, we should call the service `/world//control` and fill the request message type -`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the reset (true means everything was fine, false otherwise) +`gz.msgs.WorldControl`. This service returns a `gz.msgs.Boolean` with the status of the reset (true means everything was fine, false otherwise). The `WorldControl` message now contains a `reset` field for resetting the world: diff --git a/tutorials/using_components.md b/tutorials/using_components.md index 717beea566..1c91d47e46 100644 --- a/tutorials/using_components.md +++ b/tutorials/using_components.md @@ -72,3 +72,4 @@ The rest of the tutorial is case studies that walk through the usage of specific components. - \subpage jointforcecmdcomponent "JointForceCmd" +- \subpage posecomponent "Pose"