diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index efc76290ed..740b9f9d7d 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -15,7 +15,7 @@ libignition-tools-dev libignition-transport12-dev libignition-utils1-cli-dev libogre-1.9-dev -libogre-2.1-dev +libogre-2.2-dev libprotobuf-dev libprotoc-dev libsdformat13-dev diff --git a/Changelog.md b/Changelog.md index 2cfb1cc5d9..6d013fda34 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,103 @@ ## Ignition Gazebo 6.x +### Ignition Gazebo 6.8.0 (2022-04-04) + +1. ServerConfig accepts an sdf::Root DOM object + * [Pull request #1333](https://github.com/ignitionrobotics/ign-gazebo/pull/1333) + +1. Disable sensors in sensors system when battery is drained + * [Pull request #1385](https://github.com/ignitionrobotics/ign-gazebo/pull/1385) + +1. Referring to Fuel assets within a heightmap + * [Pull request #1419](https://github.com/ignitionrobotics/ign-gazebo/pull/1419) + +1. Add the Model Photo Shoot system, port of Modelpropshop plugin from Gazebo classic + * [Pull request #1331](https://github.com/ignitionrobotics/ign-gazebo/pull/1331) + +1. Distortion camera integration test + * [Pull request #1374](https://github.com/ignitionrobotics/ign-gazebo/pull/1374) + +1. Add wheel slip user command + * [Pull request #1241](https://github.com/ignitionrobotics/ign-gazebo/pull/1241) + +1. SceneBroadcaster: only send changed state information for change events + * [Pull request #1392](https://github.com/ignitionrobotics/ign-gazebo/pull/1392) + +1. Fortress: Install Ogre 2.2, simplify docker + * [Pull request #1395](https://github.com/ignitionrobotics/ign-gazebo/pull/1395) + +1. Disable tests that are expected to fail on Windows + * [Pull request #1408](https://github.com/ignitionrobotics/ign-gazebo/pull/1408) + +1. Added user command to set multiple entities + * [Pull request #1394](https://github.com/ignitionrobotics/ign-gazebo/pull/1394) + +1. Fix JointStatePublisher topic name for nested models + * [Pull request #1405](https://github.com/ignitionrobotics/ign-gazebo/pull/1405) + +1. add initial_position param to joint controller system + * [Pull request #1406](https://github.com/ignitionrobotics/ign-gazebo/pull/1406) + +1. Component inspector: refactor Pose3d C++ code into a separate class + * [Pull request #1400](https://github.com/ignitionrobotics/ign-gazebo/pull/1400) + +1. Prevent hanging when world has only non-world plugins + * [Pull request #1383](https://github.com/ignitionrobotics/ign-gazebo/pull/1383) + +1. Toggle Light visuals + * [Pull request #1387](https://github.com/ignitionrobotics/ign-gazebo/pull/1387) + +1. Disable PeerTracker.PeerTrackerStale on macOS + * [Pull request #1398](https://github.com/ignitionrobotics/ign-gazebo/pull/1398) + +1. Disable ModelCommandAPI_TEST.RgbdCameraSensor on macOS + * [Pull request #1397](https://github.com/ignitionrobotics/ign-gazebo/pull/1397) + +1. Don't mark entities with a ComponentState::NoChange component as modified + * [Pull request #1391](https://github.com/ignitionrobotics/ign-gazebo/pull/1391) + +1. Add gazebo Entity id to rendering sensor's user data + * [Pull request #1381](https://github.com/ignitionrobotics/ign-gazebo/pull/1381) + +1. Allow to turn on/off lights + * [Pull request #1343](https://github.com/ignitionrobotics/ign-gazebo/pull/1343) + +1. Added headless rendering tutorial + * [Pull request #1386](https://github.com/ignitionrobotics/ign-gazebo/pull/1386) + +1. Add xyz and rpy offset to published odometry pose + * [Pull request #1341](https://github.com/ignitionrobotics/ign-gazebo/pull/1341) + +1. Fix visualization python tutorial + * [Pull request #1377](https://github.com/ignitionrobotics/ign-gazebo/pull/1377) + +1. Populate GUI plugins that are empty + * [Pull request #1375](https://github.com/ignitionrobotics/ign-gazebo/pull/1375) + +### Ignition Gazebo 6.7.0 (2022-02-24) + +1. Added Python interfaces to some Ignition Gazebo methods + * [Pull request #1219](https://github.com/ignitionrobotics/ign-gazebo/pull/1219) + +1. Use pose multiplication instead of addition + * [Pull request #1369](https://github.com/ignitionrobotics/ign-gazebo/pull/1369) + +1. Disables Failing Buoyancy Tests on Win32 + * [Pull request #1368](https://github.com/ignitionrobotics/ign-gazebo/pull/1368) + +1. Extend ShaderParam system to support loading different shader languages + * [Pull request #1335](https://github.com/ignitionrobotics/ign-gazebo/pull/1335) + +1. Populate names of colliding entities in contact points message + * [Pull request #1351](https://github.com/ignitionrobotics/ign-gazebo/pull/1351) + +1. Refactor System functionality into SystemManager + * [Pull request #1340](https://github.com/ignitionrobotics/ign-gazebo/pull/1340) + +1. GzSceneManager: Prevent crash boom when inserted from menu + * [Pull request #1371](https://github.com/ignitionrobotics/ign-gazebo/pull/1371) + ### Ignition Gazebo 6.6.0 (2022-02-24) 1. Fix accessing empty JointPosition component in lift drag plugin @@ -537,7 +634,98 @@ ## Ignition Gazebo 5.x -### Ignition Gazebo 5.X.X (202X-XX-XX) +### Ignition Gazebo 5.4.0 (2022-03-31) + +1. Add the Model Photo Shoot system, port of Modelpropshop plugin from Gazebo classic + * [Pull request #1331](https://github.com/ignitionrobotics/ign-gazebo/pull/1331) + +1. Add wheel slip user command + * [Pull request #1241](https://github.com/ignitionrobotics/ign-gazebo/pull/1241) + +1. Added user command to set multiple entity poses + * [Pull request #1394](https://github.com/ignitionrobotics/ign-gazebo/pull/1394) + +1. Component inspector: refactor Pose3d C++ code into a separate class + * [Pull request #1400](https://github.com/ignitionrobotics/ign-gazebo/pull/1400) + +1. Toggle Light visuals + * [Pull request #1387](https://github.com/ignitionrobotics/ign-gazebo/pull/1387) + +1. Allow to turn on/off lights + * [Pull request #1343](https://github.com/ignitionrobotics/ign-gazebo/pull/1343) + +1. Added more sensor properties to scene/info topic + * [Pull request #1344](https://github.com/ignitionrobotics/ign-gazebo/pull/1344) + +1. JointStatePublisher publish parent, child and axis data + * [Pull request #1345](https://github.com/ignitionrobotics/ign-gazebo/pull/1345) + +1. Fixed light GUI component inspector + * [Pull request #1337](https://github.com/ignitionrobotics/ign-gazebo/pull/1337) + +1. Fix `UNIT_SdfGenerator_TEST` + * [Pull request #1319](https://github.com/ignitionrobotics/ign-gazebo/pull/1319) + +1. Add elevator system + * [Pull request #535](https://github.com/ignitionrobotics/ign-gazebo/pull/535) + +1. Removed unused variables in shapes plugin + * [Pull request #1321](https://github.com/ignitionrobotics/ign-gazebo/pull/1321) + +1. Log an error if JointPositionController cannot find the joint. (citadel retarget) + * [Pull request #1314](https://github.com/ignitionrobotics/ign-gazebo/pull/1314) + +1. Buoyancy: fix center of volume's reference frame + * [Pull request #1302](https://github.com/ignitionrobotics/ign-gazebo/pull/1302) + +1. Remove EachNew calls from sensor PreUpdates + * [Pull request #1281](https://github.com/ignitionrobotics/ign-gazebo/pull/1281) + +1. Prevent GzScene3D 💥 if another scene is already loaded + * [Pull request #1294](https://github.com/ignitionrobotics/ign-gazebo/pull/1294) + +1. Cleanup update call for non-rendering sensors + * [Pull request #1282](https://github.com/ignitionrobotics/ign-gazebo/pull/1282) + +1. Documentation Error + * [Pull request #1285](https://github.com/ignitionrobotics/ign-gazebo/pull/1285) + +1. Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. + * [Pull request #1229](https://github.com/ignitionrobotics/ign-gazebo/pull/1229) + +1. Add project() call to examples + * [Pull request #1274](https://github.com/ignitionrobotics/ign-gazebo/pull/1274) + +1. Implement `/server_control::stop` + * [Pull request #1240](https://github.com/ignitionrobotics/ign-gazebo/pull/1240) + +1. 👩‍🌾 Make depth camera tests more robust (#897) + * [Pull request #897) (#1257](https://github.com/ignitionrobotics/ign-gazebo/pull/897) (#1257) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Skip failing Windows tests + * [Pull request #1205](https://github.com/ignitionrobotics/ign-gazebo/pull/1205) + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + * [Pull request #1259](https://github.com/ignitionrobotics/ign-gazebo/pull/1259) + * [Pull request #1408](https://github.com/ignitionrobotics/ign-gazebo/pull/1408) + +1. Configurable joint state publisher's topic + * [Pull request #1076](https://github.com/ignitionrobotics/ign-gazebo/pull/1076) + +1. Thruster plugin: add tests and velocity control + * [Pull request #1190](https://github.com/ignitionrobotics/ign-gazebo/pull/1190) + +1. Limit thruster system's input thrust cmd + * [Pull request #1318](https://github.com/ignitionrobotics/ign-gazebo/pull/1318) ### Ignition Gazebo 5.3.0 (2021-11-12) diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base index e84976b01d..ac03a69978 100644 --- a/docker/Dockerfile.base +++ b/docker/Dockerfile.base @@ -11,10 +11,6 @@ RUN scripts/enable_ign_stable.sh COPY docker/scripts/install_common_deps.sh scripts/install_common_deps.sh RUN scripts/install_common_deps.sh -# This is not strictly necessary, but makes things faster down the line. -COPY docker/scripts/install_ign_deps.sh scripts/install_ign_deps.sh -RUN scripts/install_ign_deps.sh - COPY docker/scripts/enable_gcc8.sh scripts/enable_gcc8.sh RUN scripts/enable_gcc8.sh diff --git a/docker/scripts/install_ign_deps.sh b/docker/scripts/install_ign_deps.sh deleted file mode 100755 index 63948f5fb6..0000000000 --- a/docker/scripts/install_ign_deps.sh +++ /dev/null @@ -1,82 +0,0 @@ -#!/bin/bash - -set -o errexit -set -o verbose - -sudo apt-get update - -# Things that are used all over the ign stack -sudo apt-get install --no-install-recommends -y \ - doxygen \ - libbullet-dev \ - libtinyxml2-dev \ - libprotoc-dev libprotobuf-dev \ - protobuf-compiler \ - ruby-ronn \ - ruby-dev \ - swig \ - uuid-dev - -# ign-common dependencies -sudo apt-get install --no-install-recommends -y \ - libavcodec-dev \ - libavdevice-dev \ - libavformat-dev \ - libavutil-dev \ - libfreeimage-dev \ - libgts-dev \ - libswscale-dev - -# ign-gui dependencies -sudo apt-get install --no-install-recommends -y \ - qtbase5-dev \ - qtdeclarative5-dev \ - qtquickcontrols2-5-dev \ - qml-module-qtquick2 \ - qml-module-qtquick-controls \ - qml-module-qtquick-controls2 \ - qml-module-qtquick-dialogs \ - qml-module-qtquick-layouts \ - qml-module-qt-labs-folderlistmodel \ - qml-module-qt-labs-settings \ - qml-module-qtgraphicaleffects - -# ign-rendering dependencies -sudo apt-get install --no-install-recommends -y \ - libogre-1.9-dev \ - libogre-2.1-dev \ - libglew-dev \ - libfreeimage-dev \ - freeglut3-dev \ - libxmu-dev \ - libxi-dev - -# ign-transport dependencies -sudo apt-get install --no-install-recommends -y \ - libzmq3-dev \ - libsqlite3-dev - -# SDFormat dependencies -sudo apt-get install --no-install-recommends -y \ - libtinyxml-dev libxml2-dev - -# ign-fuel_tools dependencies -sudo apt-get install --no-install-recommends -y \ - libcurl4-openssl-dev libjsoncpp-dev libzip-dev curl libyaml-dev - -# ign-physics dependencies -sudo apt-get install --no-install-recommends -y \ - libeigen3-dev \ - libdart-collision-ode-dev \ - libdart-dev \ - libdart-external-ikfast-dev \ - libdart-external-odelcpsolver-dev \ - libdart-utils-urdf-dev \ - libbenchmark-dev - -# ign-gazebo dependencies -sudo apt-get install --no-install-recommends -y \ - qml-module-qtqml-models2 - -sudo apt-get clean && sudo rm -rf /var/lib/apt/lists/* - diff --git a/examples/worlds/empty_gui.sdf b/examples/worlds/empty_gui.sdf new file mode 100644 index 0000000000..84b5e172c4 --- /dev/null +++ b/examples/worlds/empty_gui.sdf @@ -0,0 +1,210 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.0 0.6 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + diff --git a/examples/worlds/fuel.sdf b/examples/worlds/fuel.sdf index f18d586932..4d50302e1a 100644 --- a/examples/worlds/fuel.sdf +++ b/examples/worlds/fuel.sdf @@ -107,5 +107,54 @@ + + + true + + + + + false + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/dirt_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/grass_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fungus_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + 28 + 6 + + + 35 + 18 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png + 257 257 50 + 0 0 -28 + + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png + 257 257 50 + 0 0 -28 + + + + + + diff --git a/examples/worlds/lights.sdf b/examples/worlds/lights.sdf index f1910f0ac7..61d2d92fd1 100644 --- a/examples/worlds/lights.sdf +++ b/examples/worlds/lights.sdf @@ -52,6 +52,7 @@ 0.01 false + false diff --git a/examples/worlds/model_photo_shoot.sdf b/examples/worlds/model_photo_shoot.sdf new file mode 100644 index 0000000000..84ec5b913d --- /dev/null +++ b/examples/worlds/model_photo_shoot.sdf @@ -0,0 +1,55 @@ + + + + + 0 0 0 + + + + ogre2 + 1, 1, 1 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + false + + + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + + + diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index a975cc7bc4..7311d092e8 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,23 @@ namespace ignition /// configuration. class IGNITION_GAZEBO_VISIBLE ServerConfig { + /// \brief Type of SDF source. + public: enum class SourceType + { + // No source specified. + kNone, + + // The source is an SDF Root object. + kSdfRoot, + + // The source is an SDF file. + kSdfFile, + + // The source is an SDF string. + kSdfString, + }; + + class PluginInfoPrivate; /// \brief Information about a plugin that should be loaded by the /// server. @@ -175,6 +193,17 @@ namespace ignition /// \return The full contents of the SDF string, or empty string. public: std::string SdfString() const; + /// \brief Set the SDF Root DOM object. The sdf::Root object will take + /// precendence over ServerConfig::SdfString() and + /// ServerConfig::SdfFile(). + /// \param[in] _root SDF Root object to use. + public: void SetSdfRoot(const sdf::Root &_root) const; + + /// \brief Get the SDF Root DOM object. + /// \return SDF Root object to use, or std::nullopt if the sdf::Root + /// has not been set via ServerConfig::SetSdfRoot(). + public: std::optional &SdfRoot() const; + /// \brief Set the update rate in Hertz. Value <=0 are ignored. /// \param[in] _hz The desired update rate of the server in Hertz. public: void SetUpdateRate(const double &_hz); @@ -383,6 +412,10 @@ namespace ignition public: const std::chrono::time_point & Timestamp() const; + /// \brief Get the type of source + /// \return The source type. + public: SourceType Source() const; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 67e405659f..18cc6798c9 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -210,6 +210,25 @@ namespace ignition const EntityComponentManager &_ecm, bool _excludeWorld = true); + /// \brief Convert an SDF world filename string, such as "shapes.sdf", to + /// full system file path. + /// The provided SDF filename may be a Fuel URI, relative path, name + /// of an installed Gazebo world filename, or an absolute path. + /// \param[in] _sdfFile An SDF world filename such as: + /// 1. "shapes.sdf" - This is referencing an installed world file. + /// 2. "../shapes.sdf" - This is referencing a relative world file. + /// 3. "/home/user/shapes.sdf" - This is reference an absolute world + /// file. + /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" + /// This is referencing a Fuel URI. This will download the world file. + /// \param[in] _fuelResourceCache Path to a Fuel resource cache, if + /// known. + /// \return Full path to the SDF world file. An empty string is returned + /// if the file could not be found. + std::string IGNITION_GAZEBO_VISIBLE resolveSdfWorldFile( + const std::string &_sdfFilename, + const std::string &_fuelResourceCache = ""); + /// \brief Helper function to "enable" a component (i.e. create it if it /// doesn't exist) or "disable" a component (i.e. remove it if it exists). /// \param[in] _ecm Mutable reference to the ECM diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh new file mode 100644 index 0000000000..4daf7ee249 --- /dev/null +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_WHEELSLIPCMD_HH_ + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains commanded wheel slip parameters of + /// an entity in the world frame represented by msgs::WheelSlipParameters. + using WheelSlipCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WheelSlipCmd", + WheelSlipCmd) +} +} +} +} +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index bdc67fd9cb..549c1e527f 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -570,6 +570,25 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in) out.set_spot_inner_angle(_in.SpotInnerAngle().Radian()); out.set_spot_outer_angle(_in.SpotOuterAngle().Radian()); out.set_spot_falloff(_in.SpotFalloff()); + + { + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + auto header = out.mutable_header()->add_data(); + header->set_key("isLightOn"); + std::string *value = header->add_value(); + *value = std::to_string(_in.LightOn()); + } + + { + // todo(ahcorde) Use the field visualize_visual in light.proto from + // Garden on. + auto header = out.mutable_header()->add_data(); + header->set_key("visualizeVisual"); + std::string *value = header->add_value(); + *value = std::to_string(_in.Visualize()); + } + if (_in.Type() == sdf::LightType::POINT) out.set_type(msgs::Light_LightType_POINT); else if (_in.Type() == sdf::LightType::SPOT) @@ -599,6 +618,43 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in) out.SetSpotInnerAngle(math::Angle(_in.spot_inner_angle())); out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle())); out.SetSpotFalloff(_in.spot_falloff()); + + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + bool visualizeVisual = true; + for (int i = 0; i < _in.header().data_size(); ++i) + { + for (int j = 0; + j < _in.header().data(i).value_size(); ++j) + { + if (_in.header().data(i).key() == + "visualizeVisual") + { + visualizeVisual = ignition::math::parseInt( + _in.header().data(i).value(0)); + } + } + } + out.SetVisualize(visualizeVisual); + + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + bool isLightOn = true; + for (int i = 0; i < _in.header().data_size(); ++i) + { + for (int j = 0; + j < _in.header().data(i).value_size(); ++j) + { + if (_in.header().data(i).key() == + "isLightOn") + { + isLightOn = ignition::math::parseInt( + _in.header().data(i).value(0)); + } + } + } + out.SetLightOn(isLightOn); + if (_in.type() == msgs::Light_LightType_POINT) out.SetType(sdf::LightType::POINT); else if (_in.type() == msgs::Light_LightType_SPOT) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index d512f8c0e8..201f21bf01 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -1855,6 +1855,8 @@ void EntityComponentManager::SetState( components::BaseComponent *comp = this->ComponentImplementation(entity, compIter.first); + std::istringstream istr(compMsg.component()); + // Create if new if (nullptr == comp) { @@ -1868,7 +1870,6 @@ void EntityComponentManager::SetState( continue; } - std::istringstream istr(compMsg.component()); newComp->Deserialize(istr); this->CreateComponentImplementation(entity, @@ -1877,7 +1878,6 @@ void EntityComponentManager::SetState( // Update component value else { - std::istringstream istr(compMsg.component()); comp->Deserialize(istr); this->SetChanged(entity, compIter.first, _stateMsg.has_one_time_component_changes() ? @@ -1958,6 +1958,10 @@ void EntityComponentManager::SetChanged( auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_type); if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) oneTimeIter->second.erase(_entity); + + // the component state is flagged as no change, so don't mark the + // corresponding entity as one with a modified component + return; } this->dataPtr->AddModifiedComponent(_entity); diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 4305803f48..63eac65d1d 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2251,6 +2251,17 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e2, c2->TypeId())); + // Marking a component that isn't changed as unchanged again shouldn't effect + // the ecm's changed state + manager.RunClearNewlyCreatedEntities(); + EXPECT_EQ(0, manager.ChangedState().entities_size()); + manager.SetChanged(e1, c1->TypeId(), ComponentState::NoChange); + EXPECT_EQ(ComponentState::NoChange, + manager.ComponentState(e1, c1->TypeId())); + EXPECT_FALSE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); + EXPECT_EQ(0, manager.ChangedState().entities_size()); + // Mark as changed manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 36eafc23f4..5102d589ec 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -20,7 +20,7 @@ #include #include -#include +#include #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) @@ -595,7 +595,7 @@ TEST(ModelCommandAPI, MagnetometerSensor) ///////////////////////////////////////////////// // Tests `ign model -s` command with an rgbd camera. -TEST(ModelCommandAPI, RgbdCameraSensor) +TEST(ModelCommandAPI, IGN_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) { ignition::gazebo::ServerConfig serverConfig; // Using an static model to avoid any movements in the simulation. diff --git a/src/Server.cc b/src/Server.cc index 9b2a2b8448..87381ee516 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -33,31 +33,6 @@ using namespace ignition; using namespace gazebo; -////////////////////////////////////////////////// -// Getting the first .sdf file in the path -std::string findFuelResourceSdf(const std::string &_path) -{ - if (!common::exists(_path)) - return ""; - - for (common::DirIter file(_path); file != common::DirIter(); ++file) - { - std::string current(*file); - if (!common::isFile(current)) - continue; - - auto fileName = common::basename(current); - auto fileExtensionIndex = fileName.rfind("."); - auto fileExtension = fileName.substr(fileExtensionIndex + 1); - - if (fileExtension == "sdf") - { - return current; - } - } - return ""; -} - /// \brief This struct provides access to the default world. struct DefaultWorld { @@ -98,83 +73,64 @@ Server::Server(const ServerConfig &_config) sdf::Errors errors; - // Load a world if specified. Check SDF string first, then SDF file - if (!_config.SdfString().empty()) + switch (_config.Source()) { - std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) + // Load a world if specified. Check SDF string first, then SDF file + case ServerConfig::SourceType::kSdfRoot: { - msg += "File path not available.\n"; + this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); + ignmsg << "Loading SDF world from SDF DOM.\n"; + break; } - else - { - msg += "File path [" + _config.SdfFile() + "].\n"; - } - ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); - } - else if (!_config.SdfFile().empty()) - { - std::string filePath; - // Check Fuel if it's a URL - auto sdfUri = common::URI(_config.SdfFile()); - if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") + case ServerConfig::SourceType::kSdfString: { - std::string fuelCachePath; - if (this->dataPtr->fuelClient->CachedWorld(common::URI(_config.SdfFile()), - fuelCachePath)) + std::string msg = "Loading SDF string. "; + if (_config.SdfFile().empty()) { - filePath = findFuelResourceSdf(fuelCachePath); - } - else if (auto result = this->dataPtr->fuelClient->DownloadWorld( - common::URI(_config.SdfFile()), fuelCachePath)) - { - filePath = findFuelResourceSdf(fuelCachePath); + msg += "File path not available.\n"; } else { - ignwarn << "Fuel couldn't download URL [" << _config.SdfFile() - << "], error: [" << result.ReadableResult() << "]" - << std::endl; + msg += "File path [" + _config.SdfFile() + "].\n"; } + ignmsg << msg; + errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); + break; } - if (filePath.empty()) + case ServerConfig::SourceType::kSdfFile: { - common::SystemPaths systemPaths; + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); - // Worlds from environment variable - systemPaths.SetFilePathEnv(kResourcePathEnv); + if (filePath.empty()) + { + ignerr << "Failed to find world [" << _config.SdfFile() << "]" + << std::endl; + return; + } - // Worlds installed with ign-gazebo - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + ignmsg << "Loading SDF world file[" << filePath << "].\n"; - filePath = systemPaths.FindFile(_config.SdfFile()); + // \todo(nkoenig) Async resource download. + // This call can block for a long period of time while + // resources are downloaded. Blocking here causes the GUI to block with + // a black screen (search for "Async resource download" in + // 'src/gui_main.cc'. + errors = this->dataPtr->sdfRoot.Load(filePath); + break; } - if (filePath.empty()) + case ServerConfig::SourceType::kNone: + default: { - ignerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; - return; + ignmsg << "Loading default world.\n"; + // Load an empty world. + /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. + errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + break; } - - ignmsg << "Loading SDF world file[" << filePath << "].\n"; - - // \todo(nkoenig) Async resource download. - // This call can block for a long period of time while - // resources are downloaded. Blocking here causes the GUI to block with - // a black screen (search for "Async resource download" in - // 'src/gui_main.cc'. - errors = this->dataPtr->sdfRoot.Load(filePath); - } - else - { - ignmsg << "Loading default world.\n"; - // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); } if (!errors.empty()) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index fb0d9fdace..1024eb5d11 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -301,6 +301,12 @@ class ignition::gazebo::ServerConfigPrivate /// \brief is the headless mode active. public: bool isHeadlessRendering{false}; + + /// \brief Optional SDF root object. + public: std::optional sdfRoot; + + /// \brief Type of source used. + public: ServerConfig::SourceType source{ServerConfig::SourceType::kNone}; }; ////////////////////////////////////////////////// @@ -321,8 +327,10 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { + this->dataPtr->source = ServerConfig::SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -335,8 +343,10 @@ std::string ServerConfig::SdfFile() const ////////////////////////////////////////////////// bool ServerConfig::SetSdfString(const std::string &_sdfString) { + this->dataPtr->source = ServerConfig::SourceType::kSdfString; this->dataPtr->sdfFile = ""; this->dataPtr->sdfString = _sdfString; + this->dataPtr->sdfRoot = std::nullopt; return true; } @@ -697,6 +707,35 @@ const std::vector &ServerConfig::LogRecordTopics() const return this->dataPtr->logRecordTopics; } +///////////////////////////////////////////////// +void ServerConfig::SetSdfRoot(const sdf::Root &_root) const +{ + this->dataPtr->source = ServerConfig::SourceType::kSdfRoot; + this->dataPtr->sdfRoot.emplace(); + + for (uint64_t i = 0; i < _root.WorldCount(); ++i) + { + const sdf::World *world = _root.WorldByIndex(i); + if (world) + this->dataPtr->sdfRoot->AddWorld(*world); + } + + this->dataPtr->sdfFile = ""; + this->dataPtr->sdfString = ""; +} + +///////////////////////////////////////////////// +std::optional &ServerConfig::SdfRoot() const +{ + return this->dataPtr->sdfRoot; +} + +///////////////////////////////////////////////// +ServerConfig::SourceType ServerConfig::Source() const +{ + return this->dataPtr->source; +} + ///////////////////////////////////////////////// void copyElement(sdf::ElementPtr _sdf, const tinyxml2::XMLElement *_xml) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 733a55ef37..bc93120ef2 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -228,3 +228,31 @@ TEST(ServerConfig, GenerateRecordPlugin) EXPECT_EQ(plugin.Name(), "ignition::gazebo::systems::LogRecord"); } +////////////////////////////////////////////////// +TEST(ServerConfig, SdfRoot) +{ + ServerConfig config; + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kNone, config.Source()); + + config.SetSdfString("string"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_FALSE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfString, config.Source()); + + config.SetSdfFile("file"); + EXPECT_FALSE(config.SdfRoot()); + EXPECT_FALSE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfFile, config.Source()); + + sdf::Root root; + config.SetSdfRoot(root); + EXPECT_TRUE(config.SdfRoot()); + EXPECT_TRUE(config.SdfFile().empty()); + EXPECT_TRUE(config.SdfString().empty()); + EXPECT_EQ(ServerConfig::SourceType::kSdfRoot, config.Source()); +} diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 879806b2e3..a2c6d1803d 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -295,6 +295,49 @@ TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfServerConfig)) EXPECT_FALSE(server.HasEntity("bad", 1)); } +///////////////////////////////////////////////// +TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SdfRootServerConfig)) +{ + ignition::gazebo::ServerConfig serverConfig; + + serverConfig.SetSdfString(TestWorldSansPhysics::World()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_FALSE(serverConfig.SdfString().empty()); + + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "air_pressure.sdf")); + EXPECT_FALSE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + sdf::Root root; + root.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); + + // Setting the SDF Root should override the string and file. + serverConfig.SetSdfRoot(root); + + EXPECT_TRUE(serverConfig.SdfRoot()); + EXPECT_TRUE(serverConfig.SdfFile().empty()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + EXPECT_TRUE(*server.Paused()); + EXPECT_EQ(0u, *server.IterationCount()); + EXPECT_EQ(24u, *server.EntityCount()); + EXPECT_EQ(3u, *server.SystemCount()); + + EXPECT_TRUE(server.HasEntity("box")); + EXPECT_FALSE(server.HasEntity("box", 1)); + EXPECT_TRUE(server.HasEntity("sphere")); + EXPECT_TRUE(server.HasEntity("cylinder")); + EXPECT_TRUE(server.HasEntity("capsule")); + EXPECT_TRUE(server.HasEntity("ellipsoid")); + EXPECT_FALSE(server.HasEntity("bad", 0)); + EXPECT_FALSE(server.HasEntity("bad", 1)); +} + ///////////////////////////////////////////////// TEST_P(ServerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 6edd7c63d3..0481a2df21 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -175,9 +175,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Load any additional plugins from the Server Configuration this->LoadServerPlugins(this->serverConfig.Plugins()); - // If we have reached this point and no systems have been loaded, then load - // a default set of systems. - if (this->systemMgr->TotalCount() == 0) + // If we have reached this point and no world systems have been loaded, then + // load a default set of systems. + if (this->systemMgr->TotalByEntity( + worldEntity(this->entityCompMgr)).empty()) { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 17641a873c..3d525f0f23 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1532,6 +1532,30 @@ TEST_P(SimulationRunnerTest, componentId)) << componentId; } +///////////////////////////////////////////////// +TEST_P(SimulationRunnerTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadOnlyModelPlugin) ) +{ + sdf::Root rootWithout; + rootWithout.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "model_plugin_only.sdf")); + ASSERT_EQ(1u, rootWithout.WorldCount()); + + // ServerConfig will fall back to environment variable + auto config = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "server_valid2.config"); + ASSERT_EQ(true, common::setenv(gazebo::kServerConfigPathEnv, config)); + ServerConfig serverConfig; + + // Create simulation runner + auto systemLoader = std::make_shared(); + SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, + serverConfig); + + // 1 model plugin from SDF and 2 world plugins from config + ASSERT_EQ(3u, runner.SystemCount()); +} + ///////////////////////////////////////////////// TEST_P(SimulationRunnerTest, GuiInfo) { diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index f8703f098f..cca49a965b 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -39,27 +39,33 @@ namespace ignition { /// \brief Constructor /// \param[in] _systemPlugin A system loaded from a plugin. - public: explicit SystemInternal(SystemPluginPtr _systemPlugin) + /// \param[in] _entity The entity that this system is attached to. + public: explicit SystemInternal(SystemPluginPtr _systemPlugin, + Entity _entity) : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), configure(systemPlugin->QueryInterface()), reset(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), - postupdate(systemPlugin->QueryInterface()) + postupdate(systemPlugin->QueryInterface()), + parentEntity(_entity) { } /// \brief Constructor /// \param[in] _system Pointer to a system. - public: explicit SystemInternal(const std::shared_ptr &_system) + /// \param[in] _entity The entity that this system is attached to. + public: explicit SystemInternal(const std::shared_ptr &_system, + Entity _entity) : systemShared(_system), system(_system.get()), configure(dynamic_cast(_system.get())), reset(dynamic_cast(_system.get())), preupdate(dynamic_cast(_system.get())), update(dynamic_cast(_system.get())), - postupdate(dynamic_cast(_system.get())) + postupdate(dynamic_cast(_system.get())), + parentEntity(_entity) { } @@ -95,9 +101,9 @@ namespace ignition /// Will be nullptr if the System doesn't implement this interface. public: ISystemPostUpdate *postupdate = nullptr; - /// \brief Cached entity that was used to call `Configure` on the system - /// Useful for if a system needs to be reconfigured at runtime - public: Entity configureEntity = {kNullEntity}; + /// \brief Entity that the system is attached to. It's passed to the + /// system during the `Configure` call. + public: Entity parentEntity = {kNullEntity}; /// \brief Cached sdf that was used to call `Configure` on the system /// Useful for if a system needs to be reconfigured at runtime diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 53085641e4..bde885612c 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -106,7 +106,7 @@ void SystemManager::AddSystem(const SystemPluginPtr &_system, Entity _entity, std::shared_ptr _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); + this->AddSystemImpl(SystemInternal(_system, _entity), _sdf); } ////////////////////////////////////////////////// @@ -115,19 +115,18 @@ void SystemManager::AddSystem( Entity _entity, std::shared_ptr _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); + this->AddSystemImpl(SystemInternal(_system, _entity), _sdf); } ////////////////////////////////////////////////// void SystemManager::AddSystemImpl( SystemInternal _system, - Entity _entity, std::shared_ptr _sdf) { // Configure the system, if necessary if (_system.configure && this->entityCompMgr && this->eventMgr) { - _system.configure->Configure(_entity, _sdf, + _system.configure->Configure(_system.parentEntity, _sdf, *this->entityCompMgr, *this->eventMgr); } @@ -166,3 +165,20 @@ const std::vector& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; } + +////////////////////////////////////////////////// +std::vector SystemManager::TotalByEntity(Entity _entity) +{ + std::vector result; + for (auto system : this->systems) + { + if (system.parentEntity == _entity) + result.push_back(system); + } + for (auto system : this->pendingSystems) + { + if (system.parentEntity == _entity) + result.push_back(system); + } + return result; +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 89a8f89d63..f6f1e214a2 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -92,29 +92,36 @@ namespace ignition /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); - /// \brief Get an vector of all systems implementing "Configure" + /// \brief Get an vector of all active systems implementing "Configure" + /// \return Vector of systems's configure interfaces. public: const std::vector& SystemsConfigure(); - /// \brief Get an vector of all systems implementing "Reset" + /// \brief Get an vector of all active systems implementing "Reset" + /// \return Vector of systems's reset interfaces. public: const std::vector& SystemsReset(); - /// \brief Get an vector of all systems implementing "PreUpdate" + /// \brief Get an vector of all active systems implementing "PreUpdate" + /// \return Vector of systems's pre-update interfaces. public: const std::vector& SystemsPreUpdate(); - /// \brief Get an vector of all systems implementing "Update" + /// \brief Get an vector of all active systems implementing "Update" + /// \return Vector of systems's update interfaces. public: const std::vector& SystemsUpdate(); - /// \brief Get an vector of all systems implementing "PostUpdate" + /// \brief Get an vector of all active systems implementing "PostUpdate" + /// \return Vector of systems's post-update interfaces. public: const std::vector& SystemsPostUpdate(); + /// \brief Get an vector of all systems attached to a given entity. + /// \return Vector of systems. + public: std::vector TotalByEntity(Entity _entity); + /// \brief Implementation for AddSystem functions. This only adds systems /// to a queue, the actual addition is performed by `AddSystemToRunner` at /// the appropriate time. /// \param[in] _system Generic representation of a system. - /// \param[in] _entity Entity received from AddSystem. /// \param[in] _sdf SDF received from AddSystem. private: void AddSystemImpl(SystemInternal _system, - Entity _entity, std::shared_ptr _sdf); /// \brief All the systems. diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 4fc7288580..454f53e45a 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -93,7 +93,8 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); auto configSystem = std::make_shared(); - systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + Entity configEntity{123u}; + systemMgr.AddSystem(configSystem, configEntity, nullptr); // SystemManager without an ECM/EventmManager will mean no config occurs EXPECT_EQ(0, configSystem->configured); @@ -105,6 +106,7 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size()); systemMgr.ActivatePendingSystems(); EXPECT_EQ(1u, systemMgr.ActiveCount()); @@ -114,9 +116,11 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size()); auto updateSystem = std::make_shared(); - systemMgr.AddSystem(updateSystem, kNullEntity, nullptr); + Entity updateEntity{456u}; + systemMgr.AddSystem(updateSystem, updateEntity, nullptr); EXPECT_EQ(1u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); EXPECT_EQ(2u, systemMgr.TotalCount()); @@ -124,6 +128,7 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); systemMgr.ActivatePendingSystems(); EXPECT_EQ(2u, systemMgr.ActiveCount()); @@ -133,6 +138,7 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); } ///////////////////////////////////////////////// diff --git a/src/Util.cc b/src/Util.cc index 0b11cf1c5b..b603697cf4 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -21,6 +21,9 @@ #include #include +#include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -552,7 +555,80 @@ std::optional sphericalCoordinates(Entity _entity, // Return degrees return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z()); } + +////////////////////////////////////////////////// +// Getting the first .sdf file in the path +std::string findFuelResourceSdf(const std::string &_path) +{ + if (!common::exists(_path)) + return ""; + + for (common::DirIter file(_path); file != common::DirIter(); ++file) + { + std::string current(*file); + if (!common::isFile(current)) + continue; + + auto fileName = common::basename(current); + auto fileExtensionIndex = fileName.rfind("."); + auto fileExtension = fileName.substr(fileExtensionIndex + 1); + + if (fileExtension == "sdf") + { + return current; + } + } + return ""; +} + +////////////////////////////////////////////////// +std::string resolveSdfWorldFile(const std::string &_sdfFile, + const std::string &_fuelResourceCache) +{ + std::string filePath; + + // Check Fuel if it's a URL + auto sdfUri = common::URI(_sdfFile); + if (sdfUri.Scheme() == "http" || sdfUri.Scheme() == "https") + { + fuel_tools::ClientConfig config; + if (!_fuelResourceCache.empty()) + config.SetCacheLocation(_fuelResourceCache); + fuel_tools::FuelClient fuelClient(config); + + std::string fuelCachePath; + if (fuelClient.CachedWorld(common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else if (auto result = fuelClient.DownloadWorld( + common::URI(_sdfFile), fuelCachePath)) + { + filePath = findFuelResourceSdf(fuelCachePath); + } + else + { + ignwarn << "Fuel couldn't download URL [" << _sdfFile + << "], error: [" << result.ReadableResult() << "]" + << std::endl; + } + } + + if (filePath.empty()) + { + common::SystemPaths systemPaths; + + // Worlds from environment variable + systemPaths.SetFilePathEnv(kResourcePathEnv); + + // Worlds installed with ign-gazebo + systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + + filePath = systemPaths.FindFile(_sdfFile); + } + + return filePath; +} } } } - diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index ac83bf2643..3f2b4443a9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -21,6 +21,8 @@ #include #include +#include + #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Joint.hh" @@ -716,3 +718,42 @@ TEST_F(UtilTest, EnableComponent) EXPECT_FALSE(enableComponent(ecm, entity1, false)); EXPECT_EQ(nullptr, ecm.Component(entity1)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, ResolveSdfWorldFile) +{ + // Test resolving a Fuel URI + fuel_tools::ClientConfig config; + + // URI to a Fuel world. + std::string fuelUri = + "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/test world"; + + // The expect path for the local Fuel world. + std::string expectedPath = common::joinPaths( + config.CacheLocation(), "fuel.ignitionrobotics.org", + "openrobotics", "worlds", "test world"); + + // Get the Fuel world. + std::string resolvedPath = resolveSdfWorldFile(fuelUri, + config.CacheLocation()); + + // The Fuel model has not been downloaded, so it should not exist. + EXPECT_FALSE(resolvedPath.empty()); + + // The expected path should be the first part of the resolved path. The + // resolved path will have extra world version information at the end. + EXPECT_EQ(0u, resolvedPath.find(expectedPath)); + + // Now try to resolve the downloaded world file using an aboslute path + EXPECT_EQ(resolvedPath, resolveSdfWorldFile(resolvedPath)); + + // The "shapes.sdf" world file should resolve. + EXPECT_FALSE(resolveSdfWorldFile("shapes.sdf").empty()); + + // A bad absolute path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("/invalid/does_not_exist.sdf").empty()); + + // A bad relative path should return an empty string + EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); +} diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 367278b24d..d4bc795abc 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -1,4 +1,8 @@ gz_add_gui_plugin(ComponentInspector - SOURCES ComponentInspector.cc - QT_HEADERS ComponentInspector.hh + SOURCES + ComponentInspector.cc + Pose3d.cc + QT_HEADERS + ComponentInspector.hh + Pose3d.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 5dbfff76b4..5bf8124493 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -24,7 +24,6 @@ #include #include #include -#include #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/AngularAcceleration.hh" @@ -56,8 +55,6 @@ #include "ignition/gazebo/components/PerformerAffinity.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" -#include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/SelfCollide.hh" @@ -75,6 +72,7 @@ #include "ignition/gazebo/gui/GuiEvents.hh" #include "ComponentInspector.hh" +#include "Pose3d.hh" namespace ignition::gazebo { @@ -109,31 +107,19 @@ namespace ignition::gazebo /// \brief Transport node for making command requests public: transport::Node node; + + /// \brief A map of component types to the function used to update it. + public: std::map + updateViewCbs; + + /// \brief Handles all components displayed as a 3D pose. + public: std::unique_ptr pose3d; }; } using namespace ignition; using namespace gazebo; -////////////////////////////////////////////////// -template<> -void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data) -{ - if (nullptr == _item) - return; - - _item->setData(QString("Pose3d"), - ComponentsModel::RoleNames().key("dataType")); - _item->setData(QList({ - QVariant(_data.Pos().X()), - QVariant(_data.Pos().Y()), - QVariant(_data.Pos().Z()), - QVariant(_data.Rot().Roll()), - QVariant(_data.Rot().Pitch()), - QVariant(_data.Rot().Yaw()) - }), ComponentsModel::RoleNames().key("data")); -} - ////////////////////////////////////////////////// template<> void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data) @@ -155,6 +141,36 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data) lightType = 2; } + bool visualizeVisual = true; + for (int i = 0; i < _data.header().data_size(); ++i) + { + for (int j = 0; + j < _data.header().data(i).value_size(); ++j) + { + if (_data.header().data(i).key() == + "visualizeVisual") + { + visualizeVisual = ignition::math::parseInt( + _data.header().data(i).value(0)); + } + } + } + + bool isLightOn = true; + for (int i = 0; i < _data.header().data_size(); ++i) + { + for (int j = 0; + j < _data.header().data(i).value_size(); ++j) + { + if (_data.header().data(i).key() == + "isLightOn") + { + isLightOn = ignition::math::parseInt( + _data.header().data(i).value(0)); + } + } + } + _item->setData(QString("Light"), ComponentsModel::RoleNames().key("dataType")); _item->setData(QList({ @@ -178,7 +194,9 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data) QVariant(_data.spot_outer_angle()), QVariant(_data.spot_falloff()), QVariant(_data.intensity()), - QVariant(lightType) + QVariant(lightType), + QVariant(isLightOn), + QVariant(visualizeVisual) }), ComponentsModel::RoleNames().key("data")); } @@ -454,6 +472,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Connect model this->Context()->setContextProperty( "ComponentsModel", &this->dataPtr->componentsModel); + + // Type-specific handlers + this->dataPtr->pose3d = std::make_unique(this); } ////////////////////////////////////////////////// @@ -737,12 +758,6 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } - else if (typeId == components::Pose::typeId) - { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); - } else if (typeId == components::RenderEngineGuiPlugin::typeId) { auto comp = _ecm.Component( @@ -864,18 +879,10 @@ void ComponentInspector::Update(const UpdateInfo &, setUnit(item, "m/s"); } } - else if (typeId == components::WorldPose::typeId) + else if (this->dataPtr->updateViewCbs.find(typeId) != + this->dataPtr->updateViewCbs.end()) { - auto comp = _ecm.Component(this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); - } - else if (typeId == components::WorldPoseCmd::typeId) - { - auto comp = _ecm.Component( - this->dataPtr->entity); - if (comp) - setData(item, comp->Data()); + this->dataPtr->updateViewCbs[typeId](_ecm, item); } else if (typeId == components::Material::typeId) { @@ -909,6 +916,13 @@ void ComponentInspector::Update(const UpdateInfo &, } } +///////////////////////////////////////////////// +void ComponentInspector::AddUpdateViewCb(ComponentTypeId _id, + inspector::UpdateViewCb _cb) +{ + this->dataPtr->updateViewCbs[_id] = _cb; +} + ///////////////////////////////////////////////// bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) { @@ -998,26 +1012,6 @@ void ComponentInspector::SetPaused(bool _paused) this->PausedChanged(); } -///////////////////////////////////////////////// -void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, - double _pitch, double _yaw) -{ - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) - { - if (!_result) - ignerr << "Error setting pose" << std::endl; - }; - - ignition::msgs::Pose req; - req.set_id(this->dataPtr->entity); - msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); - msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); - auto poseCmdService = "/world/" + this->dataPtr->worldName - + "/set_pose"; - this->dataPtr->node.Request(poseCmdService, req, cb); -} - ///////////////////////////////////////////////// void ComponentInspector::OnLight( double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, @@ -1025,7 +1019,8 @@ void ComponentInspector::OnLight( double _attRange, double _attLinear, double _attConstant, double _attQuadratic, bool _castShadows, double _directionX, double _directionY, double _directionZ, double _innerAngle, - double _outerAngle, double _falloff, double _intensity, int _type) + double _outerAngle, double _falloff, double _intensity, int _type, + bool _isLightOn, bool _visualizeVisual) { std::function cb = [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) @@ -1035,6 +1030,23 @@ void ComponentInspector::OnLight( }; ignition::msgs::Light req; + { + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + auto header = req.mutable_header()->add_data(); + header->set_key("isLightOn"); + std::string *value = header->add_value(); + *value = std::to_string(_isLightOn); + } + { + // todo(ahcorde) Use the field visualize_visual in light.proto from + // Garden on. + auto header = req.mutable_header()->add_data(); + header->set_key("visualizeVisual"); + std::string *value = header->add_value(); + *value = std::to_string(_visualizeVisual); + } + req.set_name(this->dataPtr->entityName); req.set_id(this->dataPtr->entity); ignition::msgs::Set(req.mutable_diffuse(), @@ -1237,6 +1249,18 @@ bool ComponentInspector::NestedModel() const return this->dataPtr->nestedModel; } +///////////////////////////////////////////////// +const std::string &ComponentInspector::WorldName() const +{ + return this->dataPtr->worldName; +} + +///////////////////////////////////////////////// +transport::Node &ComponentInspector::TransportNode() +{ + return this->dataPtr->node; +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 2b8bbd50c7..a4800795e2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -25,13 +25,15 @@ #include #include -#include #include +#include #include #include #include +#include "Types.hh" + #include Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) @@ -69,12 +71,6 @@ namespace gazebo template<> void setData(QStandardItem *_item, const std::string &_data); - /// \brief Specialized to set pose data. - /// \param[in] _item Item whose data will be set. - /// \param[in] _data Data to set. - template<> - void setData(QStandardItem *_item, const math::Pose3d &_data); - /// \brief Specialized to set light data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -228,15 +224,12 @@ namespace gazebo // Documentation inherited public: void Update(const UpdateInfo &, EntityComponentManager &) override; - /// \brief Callback in Qt thread when pose changes. - /// \param[in] _x X - /// \param[in] _y Y - /// \param[in] _z Z - /// \param[in] _roll Roll - /// \param[in] _pitch Pitch - /// \param[in] _yaw Yaw - public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, - double _roll, double _pitch, double _yaw); + /// \brief Add a callback that's called whenever there are updates from the + /// ECM to the view, for a given component type. + /// \param[in] _id The component type id + /// \param[in] _cb Function that's called when there are updates. + public: void AddUpdateViewCb(ComponentTypeId _id, + inspector::UpdateViewCb _cb); /// \brief Callback in Qt thread when specular changes. /// \param[in] _rSpecular specular red @@ -260,6 +253,8 @@ namespace gazebo /// \param[in] _falloff Falloff of the spotlight /// \param[in] _intensity Intensity of the light /// \param[in] _type light type + /// \param[in] _isLightOn is light on + /// \param[in] _visualizeVisual is visual enabled public: Q_INVOKABLE void OnLight( double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, double _rDiffuse, double _gDiffuse, @@ -267,7 +262,8 @@ namespace gazebo double _attLinear, double _attConstant, double _attQuadratic, bool _castShadows, double _directionX, double _directionY, double _directionZ, double _innerAngle, double _outerAngle, - double _falloff, double _intensity, int _type); + double _falloff, double _intensity, int _type, bool _isLightOn, + bool _visualizeVisual); /// \brief Callback in Qt thread when physics' properties change. /// \param[in] _stepSize step size @@ -368,6 +364,14 @@ namespace gazebo /// \brief Notify that paused has changed. signals: void PausedChanged(); + /// \brief Name of world entity + /// \return World name + public: const std::string &WorldName() const; + + /// \brief Node for communication + /// \return Transport node + public: transport::Node &TransportNode(); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 53a77abfd3..90a1e53dcb 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -101,12 +101,14 @@ Rectangle { _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, _attRange, _attLinear, _attConstant, _attQuadratic, _castShadows, _directionX, _directionY, _directionZ, - _innerAngle, _outerAngle, _falloff, _intensity, _type) { + _innerAngle, _outerAngle, _falloff, _intensity, _type, + _isLightOn, _visualizeVisual) { ComponentInspector.OnLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular, _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, _attRange, _attLinear, _attConstant, _attQuadratic, _castShadows, _directionX, _directionY, _directionZ, - _innerAngle, _outerAngle, _falloff, _intensity, _type) + _innerAngle, _outerAngle, _falloff, _intensity, _type, + _isLightOn, _visualizeVisual) } /* diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml index 26bc72806b..081b88972d 100644 --- a/src/gui/plugins/component_inspector/Light.qml +++ b/src/gui/plugins/component_inspector/Light.qml @@ -99,6 +99,12 @@ Rectangle { // Loaded item for intensity property var intensityItem: {} + // Loaded item for isLightOn + property var isLightOnItem: {} + + // Loaded item for visualizeVisuals + property var visualizeVisualItem: {} + // Send new light data to C++ function sendLight() { // TODO(anyone) There's a loss of precision when these values get to C++ @@ -123,7 +129,9 @@ Rectangle { outerAngleItem.value, falloffItem.value, intensityItem.value, - model.data[20] + model.data[20], + isLightOnItem.checked, + visualizeVisualItem.checked ); } @@ -248,6 +256,68 @@ Rectangle { id: grid width: parent.width + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: visualizeVisualText.width + indentation*3 + + Text { + id : visualizeVisualText + text: ' View gizmo' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + + Loader { + id: visualizeVisualLoader + anchors.fill: parent + property double numberValue: model.data[22] + sourceComponent: ignSwitch + onLoaded: { + visualizeVisualItem = visualizeVisualLoader.item + } + } + } + } + + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: isOnText.width + indentation*3 + + Text { + id : isOnText + text: ' Turn on/off' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + + Loader { + id: isOnLoader + anchors.fill: parent + property double numberValue: model.data[21] + sourceComponent: ignSwitch + onLoaded: { + isLightOnItem = isOnLoader.item + } + } + } + } + RowLayout { Rectangle { color: "transparent" diff --git a/src/gui/plugins/component_inspector/Pose3d.cc b/src/gui/plugins/component_inspector/Pose3d.cc new file mode 100644 index 0000000000..e3a9446202 --- /dev/null +++ b/src/gui/plugins/component_inspector/Pose3d.cc @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include + +#include "Pose3d.hh" + +using namespace ignition; +using namespace gazebo; +using namespace inspector; + +///////////////////////////////////////////////// +Pose3d::Pose3d(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("Pose3dImpl", this); + this->inspector = _inspector; + + this->inspector->AddUpdateViewCb(components::Pose::typeId, + std::bind(&Pose3d::UpdateView, this, + std::placeholders::_1, std::placeholders::_2)); + this->inspector->AddUpdateViewCb(components::WorldPose::typeId, + std::bind(&Pose3d::UpdateView, this, + std::placeholders::_1, std::placeholders::_2)); + this->inspector->AddUpdateViewCb(components::WorldPoseCmd::typeId, + std::bind(&Pose3d::UpdateView, this, + std::placeholders::_1, std::placeholders::_2)); +} + +///////////////////////////////////////////////// +void Pose3d::OnPose(double _x, double _y, double _z, double _roll, + double _pitch, double _yaw) +{ + std::function cb = + [](const msgs::Boolean &, const bool _result) + { + if (!_result) + ignerr << "Error setting pose" << std::endl; + }; + + msgs::Pose req; + req.set_id(this->inspector->GetEntity()); + msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z)); + msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw)); + std::string poseCmdService("/world/" + this->inspector->WorldName() + + "/set_pose"); + this->inspector->TransportNode().Request(poseCmdService, req, cb); +} diff --git a/src/gui/plugins/component_inspector/Pose3d.hh b/src/gui/plugins/component_inspector/Pose3d.hh new file mode 100644 index 0000000000..890cac2590 --- /dev/null +++ b/src/gui/plugins/component_inspector/Pose3d.hh @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_ + +#include + +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/EntityComponentManager.hh" + +#include "ComponentInspector.hh" +#include "Types.hh" + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +class ComponentInspector; +namespace inspector +{ + /// \brief Handles components that are displayed as a 3D pose: + /// * `components::Pose` + /// * `components::WorldPose` + /// * `components::WorldPoseCmd` + class Pose3d : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit Pose3d(ComponentInspector *_inspector); + + /// \brief Callback when there are ECM updates. + /// \param[in] _ecm Immutable reference to the ECM. + /// \param[in] _item Item to update. + /// \tparam ComponentType Type of component being updated. + public: + template + void UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item) + { + if (nullptr == _item) + return; + + auto comp = _ecm.Component(this->inspector->GetEntity()); + if (nullptr == comp) + return; + + auto pose = comp->Data(); + + _item->setData(QString("Pose3d"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(pose.Pos().X()), + QVariant(pose.Pos().Y()), + QVariant(pose.Pos().Z()), + QVariant(pose.Rot().Roll()), + QVariant(pose.Rot().Pitch()), + QVariant(pose.Rot().Yaw()) + }), ComponentsModel::RoleNames().key("data")); + } + + /// \brief Callback in Qt thread when pose changes. + /// \param[in] _x X + /// \param[in] _y Y + /// \param[in] _z Z + /// \param[in] _roll Roll + /// \param[in] _pitch Pitch + /// \param[in] _yaw Yaw + public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, + double _roll, double _pitch, double _yaw); + + /// \brief Pointer to the component inspector. This is used to add + /// callbacks. + private: ComponentInspector *inspector{nullptr}; + }; +} +} +} +#endif diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index e9806463e4..0690edd458 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -68,7 +68,7 @@ Rectangle { // Send new pose to C++ function sendPose() { // TODO(anyone) There's a loss of precision when these values get to C++ - componentInspector.onPose( + Pose3dImpl.OnPose( xItem.value, yItem.value, zItem.value, diff --git a/src/gui/plugins/component_inspector/Types.hh b/src/gui/plugins/component_inspector/Types.hh new file mode 100644 index 0000000000..5e0b682027 --- /dev/null +++ b/src/gui/plugins/component_inspector/Types.hh @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace inspector +{ + /// \brief Function definition that a component can use + /// to update its UI elements based on changes from the ECM. + /// * _ecm Immutable reference to the ECM + /// * _item Item to be updated + /// \sa ComponentInspector::AddUpdateViewCb + using UpdateViewCb = std::function; +} +} +} +#endif + diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc index a1507b884c..ebba5b6c54 100644 --- a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc @@ -110,7 +110,7 @@ void EntityContextMenu::LoadConfig(const tinyxml2::XMLElement *) this->dataPtr->entityContextMenuHandler); if (this->title.empty()) - this->title = "EntityContextMenu"; + this->title = "Entity Context Menu"; ignition::gui::App()->findChild ()->installEventFilter(this); diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml index 66e2f56d28..98fd9afa71 100644 --- a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml @@ -15,15 +15,39 @@ * */ -import QtQuick 2.0 -import QtQuick.Controls 2.0 +import QtQuick 2.9 +import QtQuick.Controls 2.1 import QtQuick.Layouts 1.3 import RenderWindowOverlay 1.0 import IgnGazebo 1.0 as IgnGazebo -Rectangle { - visible: false - color: "transparent" +ColumnLayout { + Layout.minimumWidth: 350 + Layout.minimumHeight: 370 + anchors.fill: parent + anchors.margins: 10 + + property string message: 'Adding a right-click context menu to the 3D scene.

' + + 'For proper positioning:
    ' + + '
  1. Undock the menu
    2. ' + + '
  2. Right-click this text, and choose Settings
    3. ' + + '
  3. Hide the title bar and set height to zero


' + + 'These other plugins must also be loaded to enable all functionality:
    ' + + '
  • Move To / Follow: Camera tracking
    • ' + + '
  • Copy / Paste: Copy Paste
    • ' + + '
  • View: Visualization Capabilities
    • ' + + '
  • Remove: Gz Scene Manager
' + + Label { + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: message + } + + Item { + width: 10 + Layout.fillHeight: true + } RenderWindowOverlay { id: renderWindowOverlay diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 48c0735f10..68d8a666c0 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -17,6 +17,9 @@ #include #include +#include + +#include #include "../../GuiRunner.hh" #include "GzSceneManager.hh" @@ -67,6 +70,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Indicates whether initial visual plugins have been loaded or not. public: bool initializedVisualPlugins = false; + + /// \brief Whether the plugin was correctly initialized + public: bool initialized{false}; }; } } @@ -90,14 +96,30 @@ void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Scene Manager"; + static bool done{false}; + if (done) + { + std::string msg{"Only one GzSceneManager is supported at a time."}; + ignerr << msg << std::endl; + QQmlProperty::write(this->PluginItem(), "message", + QString::fromStdString(msg)); + return; + } + done = true; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); + + this->dataPtr->initialized = true; } ////////////////////////////////////////////////// void GzSceneManager::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { + if (!this->dataPtr->initialized) + return; + IGN_PROFILE("GzSceneManager::Update"); this->dataPtr->renderUtil.UpdateECM(_info, _ecm); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh index ab0d5f1be0..34208e0b33 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.hh +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -34,6 +34,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// This plugin doesn't instantiate a new 3D scene. Instead, it relies on /// another plugin being loaded alongside it that will create and paint the /// scene to the window, such as `ignition::gui::plugins::Scene3D`. + /// + /// Only one GzSceneManager can be used at a time. class GzSceneManager : public GuiSystem { Q_OBJECT diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qml b/src/gui/plugins/scene_manager/GzSceneManager.qml index 873da30014..576c828bc1 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.qml +++ b/src/gui/plugins/scene_manager/GzSceneManager.qml @@ -15,14 +15,30 @@ * */ -import QtQuick 2.0 -import QtQuick.Controls 2.0 +import QtQuick 2.9 +import QtQuick.Controls 2.1 import QtQuick.Layouts 1.3 -// TODO: remove invisible rectangle, see -// https://github.com/ignitionrobotics/ign-gui/issues/220 -Rectangle { - visible: false - Layout.minimumWidth: 100 - Layout.minimumHeight: 100 +GridLayout { + columns: 1 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 130 + anchors.fill: parent + anchors.margins: 10 + + property string message: 'This plugin updates a 3D scene based on information coming from the Entity-Component-Manager.' + + Label { + Layout.columnSpan: 1 + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: message + } + + Item { + Layout.columnSpan: 1 + width: 10 + Layout.fillHeight: true + } } diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index d3513888f7..921dd03242 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -21,6 +21,8 @@ #include #include +#include + #include #include #include @@ -475,6 +477,17 @@ void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Select entities"; + static bool done{false}; + if (done) + { + std::string msg{"Only one SelectEntities plugin is supported at a time."}; + ignerr << msg << std::endl; + QQmlProperty::write(this->PluginItem(), "message", + QString::fromStdString(msg)); + return; + } + done = true; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } diff --git a/src/gui/plugins/select_entities/SelectEntities.qml b/src/gui/plugins/select_entities/SelectEntities.qml index 873da30014..71cb574e50 100644 --- a/src/gui/plugins/select_entities/SelectEntities.qml +++ b/src/gui/plugins/select_entities/SelectEntities.qml @@ -15,14 +15,26 @@ * */ -import QtQuick 2.0 -import QtQuick.Controls 2.0 +import QtQuick 2.9 +import QtQuick.Controls 2.1 import QtQuick.Layouts 1.3 -// TODO: remove invisible rectangle, see -// https://github.com/ignitionrobotics/ign-gui/issues/220 -Rectangle { - visible: false - Layout.minimumWidth: 100 - Layout.minimumHeight: 100 +ColumnLayout { + Layout.minimumWidth: 350 + Layout.minimumHeight: 90 + anchors.fill: parent + anchors.margins: 10 + + property string message: 'Adding selection functionality to the 3D scene.' + + Label { + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: message + } + + Item { + width: 10 + Layout.fillHeight: true + } } diff --git a/src/gui/plugins/spawn/Spawn.cc b/src/gui/plugins/spawn/Spawn.cc index d49ecd982d..6dfb2c9d58 100644 --- a/src/gui/plugins/spawn/Spawn.cc +++ b/src/gui/plugins/spawn/Spawn.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -180,6 +181,17 @@ void Spawn::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Spawn"; + static bool done{false}; + if (done) + { + std::string msg{"Only one Spawn plugin is supported at a time."}; + ignerr << msg << std::endl; + QQmlProperty::write(this->PluginItem(), "message", + QString::fromStdString(msg)); + return; + } + done = true; + // World name from window, to construct default topics and services auto worldNames = gui::worldNames(); if (!worldNames.empty()) diff --git a/src/gui/plugins/spawn/Spawn.qml b/src/gui/plugins/spawn/Spawn.qml index 6f5999a000..129264c393 100644 --- a/src/gui/plugins/spawn/Spawn.qml +++ b/src/gui/plugins/spawn/Spawn.qml @@ -20,10 +20,14 @@ import QtQuick.Controls 2.2 import QtQuick.Dialogs 1.0 import QtQuick.Layouts 1.3 -Rectangle { - visible: false - Layout.minimumWidth: 100 - Layout.minimumHeight: 100 +ColumnLayout { + Layout.minimumWidth: 350 + Layout.minimumHeight: 110 + anchors.fill: parent + anchors.margins: 10 + + property string message: 'Adding spawn functionality to the 3D scene, ' + + 'via events and drag and drop' Connections { target: Spawn @@ -45,4 +49,15 @@ Rectangle { } standardButtons: Dialog.Ok } + + Label { + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: message + } + + Item { + width: 10 + Layout.fillHeight: true + } } diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index fee39fefcd..68d6a16733 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -2789,7 +2790,19 @@ void VisualizationCapabilities::Update(const UpdateInfo &, void VisualizationCapabilities::LoadConfig(const tinyxml2::XMLElement *) { if (this->title.empty()) - this->title = "VisualizationCapabilities"; + this->title = "Visualization capabilities"; + + static bool done{false}; + if (done) + { + std::string msg{ + "Only one Visualization capabilities plugin is supported at a time."}; + ignerr << msg << std::endl; + QQmlProperty::write(this->PluginItem(), "message", + QString::fromStdString(msg)); + return; + } + done = true; // view as transparent service this->dataPtr->viewTransparentService = "/gui/view/transparent"; diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml index 873da30014..66469fdae7 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml @@ -15,14 +15,27 @@ * */ -import QtQuick 2.0 -import QtQuick.Controls 2.0 +import QtQuick 2.9 +import QtQuick.Controls 2.1 import QtQuick.Layouts 1.3 -// TODO: remove invisible rectangle, see -// https://github.com/ignitionrobotics/ign-gui/issues/220 -Rectangle { - visible: false - Layout.minimumWidth: 100 - Layout.minimumHeight: 100 +ColumnLayout { + Layout.minimumWidth: 350 + Layout.minimumHeight: 130 + anchors.fill: parent + anchors.margins: 10 + + property string message: 'Adding visualization capabilities to the 3D ' + + 'scene such as view transparent, joints, CoM, etc.' + + Label { + Layout.fillWidth: true + wrapMode: Text.WordWrap + text: message + } + + Item { + width: 10 + Layout.fillHeight: true + } } diff --git a/src/network/PeerTracker_TEST.cc b/src/network/PeerTracker_TEST.cc index 40b351b189..fa12da149f 100644 --- a/src/network/PeerTracker_TEST.cc +++ b/src/network/PeerTracker_TEST.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include "PeerTracker.hh" #include "ignition/gazebo/EventManager.hh" @@ -143,7 +144,7 @@ TEST(PeerTracker, PeerTracker) } ////////////////////////////////////////////////// -TEST(PeerTracker, PeerTrackerStale) +TEST(PeerTracker, IGN_UTILS_TEST_DISABLED_ON_MAC(PeerTrackerStale)) { ignition::common::Console::SetVerbosity(4); EventManager eventMgr; diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index cc123ae5ff..62522936ad 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -379,6 +379,7 @@ class ignition::gazebo::RenderUtilPrivate lightEql { [](const sdf::Light &_a, const sdf::Light &_b) { return + _a.Visualize() == _b.Visualize() && _a.Type() == _b.Type() && _a.Name() == _b.Name() && _a.Diffuse() == _b.Diffuse() && @@ -2854,11 +2855,58 @@ void RenderUtilPrivate::UpdateLights( auto l = std::dynamic_pointer_cast(node); if (l) { - if (!ignition::math::equal( - l->Intensity(), - static_cast(light.second.intensity()))) + // todo(ahcorde) Use the field visualize_visual in light.proto from + // Garden on. + bool visualizeVisual = true; + for (int i = 0; i < light.second.header().data_size(); ++i) + { + for (int j = 0; + j < light.second.header().data(i).value_size(); ++j) + { + if (light.second.header().data(i).key() == + "visualizeVisual") + { + visualizeVisual = ignition::math::parseInt( + light.second.header().data(i).value(0)); + } + } + } + + rendering::VisualPtr lightVisual = + this->sceneManager.VisualById( + this->matchLightWithVisuals[light.first]); + if (lightVisual) + lightVisual->SetVisible(visualizeVisual); + + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + bool isLightOn = true; + for (int i = 0; i < light.second.header().data_size(); ++i) + { + for (int j = 0; + j < light.second.header().data(i).value_size(); ++j) + { + if (light.second.header().data(i).key() == + "isLightOn") + { + isLightOn = ignition::math::parseInt( + light.second.header().data(i).value(0)); + } + } + } + + if (isLightOn) + { + if (!ignition::math::equal( + l->Intensity(), + static_cast(light.second.intensity()))) + { + l->SetIntensity(light.second.intensity()); + } + } + else { - l->SetIntensity(light.second.intensity()); + l->SetIntensity(0); } if (light.second.has_diffuse()) { diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 983322ae64..bc71c91d8f 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -686,8 +686,8 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, } else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP) { - auto fullPath = asFullPath(_geom.HeightmapShape()->Uri(), - _geom.HeightmapShape()->FilePath()); + auto fullPath = common::findFile(asFullPath(_geom.HeightmapShape()->Uri(), + _geom.HeightmapShape()->FilePath())); if (fullPath.empty()) { ignerr << "Heightmap geometry missing URI" << std::endl; @@ -1269,6 +1269,9 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, lightVisual->SetInnerAngle(_light.SpotInnerAngle().Radian()); lightVisual->SetOuterAngle(_light.SpotOuterAngle().Radian()); } + + lightVisual->SetVisible(_light.Visualize()); + rendering::VisualPtr lightVis = std::dynamic_pointer_cast( lightVisual); lightVis->SetUserData("gazebo-entity", static_cast(_id)); @@ -1800,6 +1803,9 @@ bool SceneManager::AddSensor(Entity _gazeboId, const std::string &_sensorName, return false; } + // \todo(anyone) change to uint64_t once UserData supports this type + sensor->SetUserData("gazebo-entity", static_cast(_gazeboId)); + if (parent) { sensor->RemoveParent(); diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index e26ad30b5a..3983736a68 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -126,6 +126,7 @@ add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) +add_subdirectory(model_photo_shoot) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index fbb5a986d0..5f7ca8a8a8 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -46,13 +46,13 @@ class ignition::gazebo::systems::JointPositionControllerPrivate public: transport::Node node; /// \brief Joint Entity - public: Entity jointEntity; + public: Entity jointEntity{kNullEntity}; /// \brief Joint name public: std::string jointName; /// \brief Commanded joint position - public: double jointPosCmd; + public: double jointPosCmd{0.0}; /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; @@ -170,6 +170,12 @@ void JointPositionController::Configure(const Entity &_entity, this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); + + if (_sdf->HasElement("initial_position")) + { + this->dataPtr->jointPosCmd = _sdf->Get("initial_position"); + } + // Subscribe to commands std::string topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + @@ -206,6 +212,8 @@ void JointPositionController::Configure(const Entity &_entity, igndbg << "cmd_min: [" << cmdMin << "]" << std::endl; igndbg << "cmd_offset: [" << cmdOffset << "]" << std::endl; igndbg << "Topic: [" << topic << "]" << std::endl; + igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]" + << std::endl; } ////////////////////////////////////////////////// diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index 6c6a6c91b2..3dcaeac3f7 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -81,6 +81,9 @@ namespace systems /// `` If you wish to listen on a non-default topic you may specify it /// here, otherwise the controller defaults to listening on /// "/model//joint///cmd_pos". + /// + /// `` Initial position of a joint. Optional parameter. + /// The default value is 0. class JointPositionController : public System, public ISystemConfigure, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 58d10ee8ee..5a995b9dfd 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -161,8 +161,10 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, { topics.push_back(this->topic); } - topics.push_back(std::string("/world/") + worldName + "/model/" - + this->model.Name(_ecm) + "/joint_state"); + std::string topicStr = + topicFromScopedName(this->model.Entity(), _ecm, false) + + "/joint_state"; + topics.push_back(topicStr); this->topic = validTopic(topics); if (this->topic.empty()) diff --git a/src/systems/model_photo_shoot/CMakeLists.txt b/src/systems/model_photo_shoot/CMakeLists.txt new file mode 100644 index 0000000000..d97b0da5e7 --- /dev/null +++ b/src/systems/model_photo_shoot/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(model-photo-shoot + SOURCES + ModelPhotoShoot.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.cc b/src/systems/model_photo_shoot/ModelPhotoShoot.cc new file mode 100644 index 0000000000..25b1bded90 --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2019 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 "ModelPhotoShoot.hh" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionReset.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private ModelPhotoShoot data class. +class ignition::gazebo::systems::ModelPhotoShootPrivate +{ + /// \brief Callback for pos rendering operations. + public: void PerformPostRenderingOperations(); + + /// \brief Save a pitcture with the camera from the given pose. + public: void SavePicture (const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) const; + + /// \brief Name of the loaded model. + public: std::string modelName; + + /// \brief model + public: std::shared_ptr model; + + /// \brief model world pose + public: ignition::math::Pose3d modelPose3D; + + /// \brief Connection to pre-render event callback. + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Boolean to control we only take the pictures once. + public: bool takePicture{true}; + + /// \brief Boolean to control if joints should adopt random poses. + public: bool randomPoses{false}; + + /// \brief File to save translation and scaling info. + public: std::ofstream savingFile; +}; + +////////////////////////////////////////////////// +ModelPhotoShoot::ModelPhotoShoot() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ModelPhotoShoot::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + std::string saveDataLocation = + _sdf->Get("translation_data_file"); + if (saveDataLocation.empty()) + { + igndbg << "No data location specified, skipping translaiton data" + "saving.\n"; + } + else + { + igndbg << "Saving translation data to: " + << saveDataLocation << std::endl; + this->dataPtr->savingFile.open(saveDataLocation); + } + + if (_sdf->HasElement("random_joints_pose")) + { + this->dataPtr->randomPoses = _sdf->Get("random_joints_pose"); + } + + this->dataPtr->connection = + _eventMgr.Connect(std::bind( + &ModelPhotoShootPrivate::PerformPostRenderingOperations, + this->dataPtr.get())); + + this->dataPtr->model = std::make_shared(_entity); + this->dataPtr->modelName = this->dataPtr->model->Name(_ecm); + // Get the pose of the model + this->dataPtr->modelPose3D = + ignition::gazebo::worldPose(this->dataPtr->model->Entity(), _ecm); +} + +////////////////////////////////////////////////// +void ModelPhotoShoot::PreUpdate( + const ignition::gazebo::UpdateInfo &, + ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->dataPtr->randomPoses) + { + std::vector joints = this->dataPtr->model->Joints(_ecm); + unsigned seed = + std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine generator(seed); + for (const auto &joint : joints) + { + auto jointNameComp = _ecm.Component(joint); + if (jointNameComp) + { + auto jointType = _ecm.Component(joint)->Data(); + if (jointType != sdf::JointType::FIXED) + { + if (jointType == sdf::JointType::REVOLUTE || + jointType == sdf::JointType::PRISMATIC) + { + // Using the JointAxis component to extract the joint pose limits + auto jointAxisComp = _ecm.Component(joint); + if (jointAxisComp) + { + std::uniform_real_distribution distribution( + jointAxisComp->Data().Lower(), + jointAxisComp->Data().Upper()); + double jointPose = distribution(generator); + _ecm.SetComponentData( + joint, {jointPose}); + + // Create a JointPosition component if it doesn't exist. + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + _ecm.SetComponentData( + joint, {jointPose}); + } + + if (this->dataPtr->savingFile.is_open()) + { + this->dataPtr->savingFile << jointNameComp->Data() << ": " + << std::setprecision(17) + << jointPose << std::endl; + } + } + else + { + ignerr << "No jointAxisComp found, ignoring joint: " << + jointNameComp->Data() << std::endl; + } + } + else + { + ignerr << "Model Photo Shoot only supports single axis joints. " + "Skipping joint: "<< jointNameComp->Data() << std::endl; + } + } + else + { + igndbg << "Ignoring fixed joint: " << jointNameComp->Data() << + std::endl; + } + } + else + { + ignerr << "No jointNameComp found on entity: " << joint << + std:: endl; + } + } + // Only set random joint poses once + this->dataPtr->randomPoses = false; + } +} + +////////////////////////////////////////////////// +void ModelPhotoShootPrivate::PerformPostRenderingOperations() +{ + ignition::rendering::ScenePtr scene = + ignition::rendering::sceneFromFirstRenderEngine(); + ignition::rendering::VisualPtr modelVisual = + scene->VisualByName(this->modelName); + + ignition::rendering::VisualPtr root = scene->RootVisual(); + + if (modelVisual && this->takePicture) + { + scene->SetAmbientLight(0.3, 0.3, 0.3); + + // create directional light + ignition::rendering::DirectionalLightPtr light0 = + scene->CreateDirectionalLight(); + light0->SetDirection(-0.5, 0.5, -1); + light0->SetDiffuseColor(0.8, 0.8, 0.8); + light0->SetSpecularColor(0.5, 0.5, 0.5); + root->AddChild(light0); + + // create point light + ignition::rendering::PointLightPtr light2 = scene->CreatePointLight(); + light2->SetDiffuseColor(0.5, 0.5, 0.5); + light2->SetSpecularColor(0.5, 0.5, 0.5); + light2->SetLocalPosition(3, 5, 5); + root->AddChild(light2); + + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto camera = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") + { + // Compute the translation we have to apply to the cameras to + // center the model in the image. + ignition::math::AxisAlignedBox bbox = modelVisual->LocalBoundingBox(); + double scaling = 1.0 / bbox.Size().Max(); + ignition::math::Vector3d bboxCenter = bbox.Center(); + ignition::math::Vector3d translation = + bboxCenter + this->modelPose3D.Pos(); + if (this->savingFile.is_open()) { + this->savingFile << "Translation: " << translation << std::endl; + this->savingFile << "Scaling: " << scaling << std::endl; + } + + ignition::math::Pose3d pose; + // Perspective view + pose.Pos().Set(1.6 / scaling + translation.X(), + -1.6 / scaling + translation.Y(), + 1.2 / scaling + translation.Z()); + pose.Rot().SetFromEuler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1.png"); + + // Top view + pose.Pos().Set(0 + translation.X(), + 0 + translation.Y(), + 2.2 / scaling + translation.Z()); + pose.Rot().SetFromEuler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2.png"); + + // Front view + pose.Pos().Set(2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3.png"); + + // Side view + pose.Pos().Set(0 + translation.X(), + 2.2 / scaling + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4.png"); + + // Back view + pose.Pos().Set(-2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, 0); + SavePicture(camera, pose, "5.png"); + + this->takePicture = false; + } + } + } +} + +////////////////////////////////////////////////// +void ModelPhotoShootPrivate::SavePicture( + const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) const +{ + unsigned int width = _camera->ImageWidth(); + unsigned int height = _camera->ImageHeight(); + ignition::common::Image image; + + _camera->SetWorldPose(_pose); + auto cameraImage = _camera->CreateImage(); + _camera->Capture(cameraImage); + auto formatStr = + ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); + auto format = ignition::common::Image::ConvertPixelFormat(formatStr); + image.SetFromData(cameraImage.Data(), width, height, format); + image.SavePNG(_fileName); + + igndbg << "Saved image to [" << _fileName << "]" << std::endl; +} + +IGNITION_ADD_PLUGIN(ModelPhotoShoot, ignition::gazebo::System, + ModelPhotoShoot::ISystemConfigure, + ModelPhotoShoot::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ModelPhotoShoot, + "ignition::gazebo::systems::ModelPhotoShoot") diff --git a/src/systems/model_photo_shoot/ModelPhotoShoot.hh b/src/systems/model_photo_shoot/ModelPhotoShoot.hh new file mode 100644 index 0000000000..b059f2486e --- /dev/null +++ b/src/systems/model_photo_shoot/ModelPhotoShoot.hh @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_MODELPHOTOSHOOT_HH_ +#define IGNITION_GAZEBO_SYSTEMS_MODELPHOTOSHOOT_HH_ + +#include + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ModelPhotoShootPrivate; + + /// \brief This plugin is a port of the old ModelPropShop plugin from gazebo + /// classic. It generates 5 pictures of a model: perspective, top, front, + /// side and back. It can do it using the default position or moving the joint + /// to random positions. It allows saving the camera and joint poses so it + /// can be replicated in other systems. + /// + /// ## System Parameters + /// - - Location to save the camera and joint poses. + /// [Optional] + /// - - Set to true to take pictures with the joints in + /// random poses instead of the default ones. This option only supports + /// single axis joints. [Optional] + /// - A camera sensor must be set in the SDF file as it will be used by the + /// plugin to take the pictures. This allows the plugin user to set the + /// camera parameters as needed. [Required] + /// + /// ## Example + /// An example configuration is installed with Gazebo. The example uses + /// the Ogre2 rendering plugin to set the background color of the pictures. + /// It also includes the camera sensor that will be used along with the + /// corresponding parameters so they can be easily tunned. + /// + /// To run the example: + /// ``` + /// ign gazebo model_photo_shoot.sdf -s -r --iterations 50 + /// ``` + /// This will start gazebo, load the model take the pictures and shutdown + /// after 50 iterations. You will find the pictures in the same location you + /// run the command. + + /// \brief System that takes snapshots of an sdf model + class ModelPhotoShoot : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + + /// \brief Constructor + public: ModelPhotoShoot(); + + /// \brief Destructor + public: ~ModelPhotoShoot() override = default; + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_id, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 3a591d75f5..8bdd6f215b 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -18,6 +18,7 @@ #include "OdometryPublisher.hh" #include +#include #include #include @@ -76,6 +77,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Diff drive odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Diff drive odometry with covariance message publisher. + public: transport::Node::Publisher odomCovPub; + /// \brief Rolling mean accumulators for the linear velocity public: std::tuple linearMean; @@ -92,6 +96,12 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Current timestamp. public: math::clock::time_point lastUpdateTime; + + /// \brief Allow specifying constant xyz and rpy offsets + public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0}; + + /// \brief Gaussian noise + public: double gaussianNoise = 0.0; }; ////////////////////////////////////////////////// @@ -142,6 +152,24 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = _sdf->Get("odom_frame"); } + if (_sdf->HasElement("xyz_offset")) + { + this->dataPtr->offset.Pos() = _sdf->Get( + "xyz_offset"); + } + + if (_sdf->HasElement("rpy_offset")) + { + this->dataPtr->offset.Rot() = + ignition::math::Quaterniond(_sdf->Get( + "rpy_offset")); + } + + if (_sdf->HasElement("gaussian_noise")) + { + this->dataPtr->gaussianNoise = _sdf->Get("gaussian_noise"); + } + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) @@ -183,17 +211,38 @@ void OdometryPublisher::Configure(const Entity &_entity, // Setup odometry std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/odometry"}; + std::string odomCovTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry_with_covariance"}; + if (_sdf->HasElement("odom_topic")) odomTopic = _sdf->Get("odom_topic"); + if (_sdf->HasElement("odom_covariance_topic")) + odomCovTopic = _sdf->Get("odom_covariance_topic"); + std::string odomTopicValid {transport::TopicUtils::AsValidTopic(odomTopic)}; if (odomTopicValid.empty()) { ignerr << "Failed to generate odom topic [" << odomTopic << "]" << std::endl; - return; } - this->dataPtr->odomPub = this->dataPtr->node.Advertise( - odomTopicValid); + else + { + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopicValid); + } + + std::string odomCovTopicValid { + transport::TopicUtils::AsValidTopic(odomCovTopic)}; + if (odomCovTopicValid.empty()) + { + ignerr << "Failed to generate odom topic [" + << odomCovTopic << "]" << std::endl; + } + else + { + this->dataPtr->odomCovPub = this->dataPtr->node.Advertise< + msgs::OdometryWithCovariance>(odomCovTopicValid); + } } ////////////////////////////////////////////////// @@ -257,7 +306,8 @@ void OdometryPublisherPrivate::UpdateOdometry( return; // Get and set robotBaseFrame to odom transformation. - const math::Pose3d pose = worldPose(this->model.Entity(), _ecm); + const math::Pose3d rawPose = worldPose(this->model.Entity(), _ecm); + 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()); msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); @@ -286,9 +336,18 @@ void OdometryPublisherPrivate::UpdateOdometry( std::get<0>(this->linearMean).Push(linearVelocityX); std::get<1>(this->linearMean).Push(linearVelocityY); msg.mutable_twist()->mutable_linear()->set_x( - std::get<0>(this->linearMean).Mean()); + std::get<0>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_y( - std::get<1>(this->linearMean).Mean()); + std::get<1>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + msg.mutable_twist()->mutable_linear()->set_z( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + + msg.mutable_twist()->mutable_angular()->set_x( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); + msg.mutable_twist()->mutable_angular()->set_y( + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); } // Get velocities and roll/pitch rates assuming 3D else if (this->dimensions == 3) @@ -317,21 +376,27 @@ void OdometryPublisherPrivate::UpdateOdometry( std::get<0>(this->angularMean).Push(rollDiff / dt.count()); std::get<1>(this->angularMean).Push(pitchDiff / dt.count()); msg.mutable_twist()->mutable_linear()->set_x( - std::get<0>(this->linearMean).Mean()); + std::get<0>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_y( - std::get<1>(this->linearMean).Mean()); + std::get<1>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_linear()->set_z( - std::get<2>(this->linearMean).Mean()); + std::get<2>(this->linearMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_angular()->set_x( - std::get<0>(this->angularMean).Mean()); + std::get<0>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); msg.mutable_twist()->mutable_angular()->set_y( - std::get<1>(this->angularMean).Mean()); + std::get<1>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); } // Set yaw rate std::get<2>(this->angularMean).Push(yawDiff / dt.count()); msg.mutable_twist()->mutable_angular()->set_z( - std::get<2>(this->angularMean).Mean()); + std::get<2>(this->angularMean).Mean() + + ignition::math::Rand::DblNormal(0, this->gaussianNoise)); // Set the time stamp in the header. msg.mutable_header()->mutable_stamp()->CopyFrom( @@ -356,7 +421,73 @@ void OdometryPublisherPrivate::UpdateOdometry( return; } this->lastOdomPubTime = _info.simTime; - this->odomPub.Publish(msg); + if (this->odomPub.Valid()) + { + this->odomPub.Publish(msg); + } + + // Generate odometry with covariance message and publish it. + msgs::OdometryWithCovariance msgCovariance; + + // Set the time stamp in the header. + msgCovariance.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame ids. + frame = msgCovariance.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(odomFrame); + childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(robotBaseFrame); + + // Copy position from odometry msg. + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_x(msg.pose().position().x()); + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_y(msg.pose().position().y()); + msgCovariance.mutable_pose_with_covariance()-> + mutable_pose()->mutable_position()->set_z(msg.pose().position().z()); + + // Copy twist from odometry msg. + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_x(msg.twist().angular().x()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_y(msg.twist().angular().y()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_angular()->set_z(msg.twist().angular().z()); + + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_x(msg.twist().linear().x()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_y(msg.twist().linear().y()); + msgCovariance.mutable_twist_with_covariance()-> + mutable_twist()->mutable_linear()->set_z(msg.twist().linear().z()); + + // Populate the covariance matrix. + // Should the matrix me populated for pose as well ? + auto gn2 = this->gaussianNoise * this->gaussianNoise; + for (int i = 0; i < 36; i++) + { + if (i % 7 == 0) + { + msgCovariance.mutable_pose_with_covariance()-> + mutable_covariance()->add_data(gn2); + msgCovariance.mutable_twist_with_covariance()-> + mutable_covariance()->add_data(gn2); + } + else + { + msgCovariance.mutable_pose_with_covariance()-> + mutable_covariance()->add_data(0); + msgCovariance.mutable_twist_with_covariance()-> + mutable_covariance()->add_data(0); + } + } + if (this->odomCovPub.Valid()) + { + this->odomCovPub.Publish(msgCovariance); + } } IGNITION_ADD_PLUGIN(OdometryPublisher, diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 832d3ac301..7fdab8253b 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -53,9 +53,25 @@ namespace systems /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/odometry`. /// + /// ``: Custom topic on which this system will publish + /// odometry with covariance messages. This element is optional, and the + /// default value is `/model/{name_of_model}/odometry_with_covariance`. + /// /// ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. + /// + /// ``: Position offset relative to the body fixed frame, the + /// default value is 0 0 0. This offset will be added to the odometry + /// message. + /// + /// ``: Rotation offset relative to the body fixed frame, the + /// default value is 0 0 0. This offset will be added to the odometry + /// message. + /// + /// ``: Standard deviation of the Gaussian noise to be added + /// to pose and twist messages. This element is optional, and the default + /// value is 0. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1c113712e8..094fafa3f1 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1284,8 +1284,8 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) return true; } - auto fullPath = asFullPath(heightmapSdf->Uri(), - heightmapSdf->FilePath()); + auto fullPath = common::findFile(asFullPath(heightmapSdf->Uri(), + heightmapSdf->FilePath())); if (fullPath.empty()) { ignerr << "Heightmap geometry missing URI" << std::endl; diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 800d338c35..ad168d2811 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -329,11 +329,16 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, set(this->dataPtr->stepMsg.mutable_stats(), _info); - // Publish full state if there are change events - if (changeEvent || this->dataPtr->stateServiceRequest) + // Publish full state if it has been explicitly requested + if (this->dataPtr->stateServiceRequest) { _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); } + // Publish the changed state if a change occurred to the ECS + else if (changeEvent) + { + _manager.ChangedState(*this->dataPtr->stepMsg.mutable_state()); + } // Otherwise publish just periodic change components when running else if (!_info.paused) { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 0d678ecc75..265fc49aa9 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -42,9 +42,11 @@ #include #include "ignition/gazebo/components/Atmosphere.hh" +#include "ignition/gazebo/components/BatterySoC.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/RenderEngineServerHeadless.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/RgbdCamera.hh" @@ -182,11 +184,34 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Stop the rendering thread public: void Stop(); + /// \brief Update battery state of sensors in model + /// \param[in] _ecm Entity component manager + public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; /// \brief Use to optionally set the ambient light. public: std::optional ambientLight; + + /// \brief A map between model entity ids in the ECM to its battery state + /// True means has charge, false means drained + public: std::unordered_map modelBatteryState; + + /// \brief A map between model entity ids in the ECM to whether its battery + /// state has changed. + /// True means has charge, false means drained + public: std::unordered_map modelBatteryStateChanged; + + /// \brief A map of sensor ids to their active state + public: std::map sensorStateChanged; + + /// \brief Disable sensors if parent model's battery is drained + /// Affects sensors that are in nested models + public: bool disableOnDrainedBattery = false; + + /// \brief Mutex to protect access to sensorStateChanged + public: std::mutex sensorStateMutex; }; ////////////////////////////////////////////////// @@ -246,6 +271,23 @@ void SensorsPrivate::RunOnce() if (!this->activeSensors.empty()) { + // disable sensors that are out of battery or re-enable sensors that are + // being charged + if (this->disableOnDrainedBattery) + { + std::unique_lock lock2(this->sensorStateMutex); + for (const auto &sensorIt : this->sensorStateChanged) + { + sensors::Sensor *s = + this->sensorManager.Sensor(sensorIt.first); + if (s) + { + s->SetActive(sensorIt.second); + } + } + this->sensorStateChanged.clear(); + } + this->sensorMaskMutex.lock(); // Check the active sensors against masked sensors. // @@ -411,6 +453,11 @@ void Sensors::Configure(const Entity &/*_id*/, std::string engineName = _sdf->Get("render_engine", "ogre2").first; + // get whether or not to disable sensor when model battery is drained + this->dataPtr->disableOnDrainedBattery = + _sdf->Get("disable_on_drained_battery", + this->dataPtr-> disableOnDrainedBattery).first; + // Get the background color, if specified. if (_sdf->HasElement("background_color")) this->dataPtr->backgroundColor = _sdf->Get("background_color"); @@ -494,7 +541,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } - { std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && @@ -550,6 +596,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (!activeSensors.empty() || this->dataPtr->renderUtil.PendingSensors() > 0) { + if (this->dataPtr->disableOnDrainedBattery) + this->dataPtr->UpdateBatteryState(_ecm); + std::unique_lock lock(this->dataPtr->renderMutex); this->dataPtr->renderCv.wait(lock, [this] { return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); @@ -567,6 +616,67 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } } + +////////////////////////////////////////////////// +void SensorsPrivate::UpdateBatteryState(const EntityComponentManager &_ecm) +{ + // Battery state + _ecm.Each( + [&](const Entity & _entity, const components::BatterySoC *_bat) + { + bool hasCharge = _bat->Data() > 0; + auto stateIt = + this->modelBatteryState.find(_ecm.ParentEntity(_entity)); + if (stateIt != this->modelBatteryState.end()) + { + // detect a change in battery charge state + if (stateIt->second != hasCharge) + { + this->modelBatteryStateChanged[_ecm.ParentEntity(_entity)] = + hasCharge; + } + } + this->modelBatteryState[_ecm.ParentEntity(_entity)] = hasCharge; + return true; + }); + + // disable sensor if parent model is out of battery or re-enable sensor + // if battery is charging + for (const auto & modelIt : this->modelBatteryStateChanged) + { + // check if sensor is part of this model + for (const auto & sensorIt : this->entityToIdMap) + { + // parent link + auto parentLinkComp = + _ecm.Component(sensorIt.first); + if (!parentLinkComp) + continue; + + // parent model + auto parentModelComp = _ecm.Component( + parentLinkComp->Data()); + if (!parentModelComp) + continue; + + // keep going up the tree in case sensor is in a nested model + while (parentModelComp) + { + auto parentEnt = parentModelComp->Data(); + if (parentEnt == modelIt.first) + { + std::unique_lock lock(this->sensorStateMutex); + // sensor is part of model - update its active state + this->sensorStateChanged[sensorIt.second] = modelIt.second; + break; + } + parentModelComp = _ecm.Component(parentEnt); + } + } + } + this->modelBatteryStateChanged.clear(); +} + ////////////////////////////////////////////////// std::string Sensors::CreateSensor(const Entity &_entity, const sdf::Sensor &_sdf, const std::string &_parentName) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index e871b9b180..87e317bc6c 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -47,6 +47,9 @@ namespace systems /// - `` Color used for the scene's ambient light. This /// will override the ambient value specified in a world's SDF /// element. This ambient light is used by sensors, not the GUI. + /// - `` Disable sensors if the model's + /// battery plugin charge reaches zero. Sensors that are in nested + /// models are also affected. /// /// \TODO(louise) Have one system for all sensors, or one per /// sensor / sensor type? diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index e37928eeb9..9ac7892eaa 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -22,11 +22,14 @@ #include #include #include +#include #include #include +#include #include #include +#include #include #include @@ -42,6 +45,8 @@ #include "ignition/common/Profiler.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/Link.hh" @@ -52,15 +57,19 @@ #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/components/SphericalCoordinates.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/World.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -129,18 +138,6 @@ Entity topLevelEntityFromMessage(const EntityComponentManager &_ecm, return kNullEntity; } -/// \brief Pose3d equality comparison function. -/// \param[in] _a A pose to compare -/// \param[in] _b Another pose to compare -bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b) -{ - return _a.Pos().Equal(_b.Pos(), 1e-6) && - math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && - math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && - math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && - math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); -} - /// \brief This class is passed to every command and contains interfaces that /// can be shared among all commands. For example, all create and remove /// commands can use the `creator` object. @@ -230,7 +227,50 @@ class LightCommand : public UserCommandBase public: std::function lightEql { [](const msgs::Light &_a, const msgs::Light &_b) { + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + auto getVisualizeVisual = [](const msgs::Light &_light) -> bool + { + bool visualizeVisual = true; + for (int i = 0; i < _light.header().data_size(); ++i) + { + for (int j = 0; + j < _light.header().data(i).value_size(); ++j) + { + if (_light.header().data(i).key() == + "visualizeVisual") + { + visualizeVisual = ignition::math::parseInt( + _light.header().data(i).value(0)); + } + } + } + return visualizeVisual; + }; + + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + auto getIsLightOn = [](const msgs::Light &_light) -> bool + { + bool isLightOn = true; + for (int i = 0; i < _light.header().data_size(); ++i) + { + for (int j = 0; + j < _light.header().data(i).value_size(); ++j) + { + if (_light.header().data(i).key() == + "isLightOn") + { + isLightOn = ignition::math::parseInt( + _light.header().data(i).value(0)); + } + } + } + return isLightOn; + }; return + getVisualizeVisual(_a) == getVisualizeVisual(_b) && + getIsLightOn(_a) == getIsLightOn(_b) && _a.type() == _b.type() && _a.name() == _b.name() && math::equal( @@ -295,6 +335,19 @@ class PoseCommand : public UserCommandBase public: bool Execute() final; }; +/// \brief Command to update an entity's pose transform. +class PoseVectorCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg pose_v message. + /// \param[in] _iface Pointer to user commands interface. + public: PoseVectorCommand(msgs::Pose_V *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; + /// \brief Command to modify the physics parameters of a simulation. class PhysicsCommand : public UserCommandBase { @@ -404,6 +457,48 @@ class VisualCommand : public UserCommandBase aMaterial.emissive().a(), bMaterial.emissive().a(), 1e-6f); }}; }; + +/// \brief Command to modify a wheel entity from simulation. +class WheelSlipCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the wheel slip parameters. + /// \param[in] _iface Pointer to user commands interface. + public: WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief WheelSlip equality comparision function + public: std::function + wheelSlipEql { + []( + const msgs::WheelSlipParametersCmd &_a, + const msgs::WheelSlipParametersCmd &_b) + { + return + ( + ( + _a.entity().id() != kNullEntity && + _a.entity().id() == _b.entity().id() + ) || + ( + _a.entity().name() == _b.entity().name() && + _a.entity().type() == _b.entity().type() + ) + ) && + math::equal( + _a.slip_compliance_lateral(), + _b.slip_compliance_lateral(), + 1e-6) && + math::equal( + _a.slip_compliance_longitudinal(), + _b.slip_compliance_longitudinal(), + 1e-6); + }}; +}; } } } @@ -454,6 +549,13 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res); + /// \brief Callback for pose_v service + /// \param[in] _req Request containing pose update of several entities. + /// \param[out] _res True if message successfully received and queued. + /// It does not mean that the entity will be successfully moved. + /// \return True if successful. + public: bool PoseVectorService(const msgs::Pose_V &_req, msgs::Boolean &_res); + /// \brief Callback for physics service /// \param[in] _req Request containing updates to the physics parameters. /// \param[out] _res True if message successfully received and queued. @@ -492,6 +594,16 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); + /// \brief Callback for wheel slip service + /// \param[in] _req Request containing wheel slip parameter updates of an + /// entity. + /// \param[out] _res True if message sucessfully received and queued. + /// It does not mean that the wheel slip parameters will be successfully + /// updated. + /// \return True if successful. + public: bool WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -505,6 +617,26 @@ class ignition::gazebo::systems::UserCommandsPrivate public: std::mutex pendingMutex; }; +/// \brief Pose3d equality comparison function. +/// \param[in] _a A pose to compare +/// \param[in] _b Another pose to compare +bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b) +{ + return _a.Pos().Equal(_b.Pos(), 1e-6) && + math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && + math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && + math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && + math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); +} + +/// \brief Update pose for a specific pose message +/// \param[in] _req Message containing new pose +/// \param[in] _iface Pointer to user commands interface. +/// \return True if successful. +bool updatePose( + const msgs::Pose &_req, + std::shared_ptr _iface); + ////////////////////////////////////////////////// UserCommands::UserCommands() : System(), dataPtr(std::make_unique()) @@ -598,6 +730,14 @@ void UserCommands::Configure(const Entity &_entity, ignmsg << "Pose service on [" << poseService << "]" << std::endl; + // Pose vector service + std::string poseVectorService{ + "/world/" + worldName + "/set_pose_vector"}; + this->dataPtr->node.Advertise(poseVectorService, + &UserCommandsPrivate::PoseVectorService, this->dataPtr.get()); + + ignmsg << "Pose service on [" << poseVectorService << "]" << std::endl; + // Light service std::string lightService{"/world/" + validWorldName + "/light_config"}; this->dataPtr->node.Advertise(lightService, @@ -651,6 +791,14 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::VisualService, this->dataPtr.get()); ignmsg << "Material service on [" << visualService << "]" << std::endl; + + // Wheel slip service + std::string wheelSlipService + {"/world/" + validWorldName + "/wheel_slip"}; + this->dataPtr->node.Advertise(wheelSlipService, + &UserCommandsPrivate::WheelSlipService, this->dataPtr.get()); + + ignmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -796,6 +944,25 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::PoseVectorService(const msgs::Pose_V &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::EnableCollisionService(const msgs::Entity &_req, msgs::Boolean &_res) @@ -870,6 +1037,25 @@ bool UserCommandsPrivate::VisualService(const msgs::Visual &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::WheelSlipService( + const msgs::WheelSlipParametersCmd &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::SphericalCoordinatesService( const msgs::SphericalCoordinates &_req, msgs::Boolean &_res) @@ -1302,59 +1488,94 @@ bool LightCommand::Execute() } ////////////////////////////////////////////////// -PoseCommand::PoseCommand(msgs::Pose *_msg, - std::shared_ptr &_iface) - : UserCommandBase(_msg, _iface) -{ -} - -////////////////////////////////////////////////// -bool PoseCommand::Execute() +bool updatePose( + const msgs::Pose &_poseMsg, + std::shared_ptr _iface) { - auto poseMsg = dynamic_cast(this->msg); - if (nullptr == poseMsg) - { - ignerr << "Internal error, null create message" << std::endl; - return false; - } - // Check the name of the entity being spawned - std::string entityName = poseMsg->name(); + std::string entityName = _poseMsg.name(); Entity entity = kNullEntity; // TODO(anyone) Update pose message to use Entity, with default ID null - if (poseMsg->id() != kNullEntity && poseMsg->id() != 0) + if (_poseMsg.id() != kNullEntity && _poseMsg.id() != 0) { - entity = poseMsg->id(); + entity = _poseMsg.id(); } else if (!entityName.empty()) { - entity = this->iface->ecm->EntityByComponents(components::Name(entityName), - components::ParentEntity(this->iface->worldEntity)); + entity = _iface->ecm->EntityByComponents(components::Name(entityName), + components::ParentEntity(_iface->worldEntity)); } - if (!this->iface->ecm->HasEntity(entity)) + if (!_iface->ecm->HasEntity(entity)) { - ignerr << "Unable to update the pose for entity id:[" << poseMsg->id() + ignerr << "Unable to update the pose for entity id:[" << _poseMsg.id() << "], name[" << entityName << "]" << std::endl; return false; } auto poseCmdComp = - this->iface->ecm->Component(entity); + _iface->ecm->Component(entity); if (!poseCmdComp) { - this->iface->ecm->CreateComponent( - entity, components::WorldPoseCmd(msgs::Convert(*poseMsg))); + _iface->ecm->CreateComponent( + entity, components::WorldPoseCmd(msgs::Convert(_poseMsg))); } else { /// \todo(anyone) Moving an object is not captured in a log file. - auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), pose3Eql) ? + auto state = poseCmdComp->SetData(msgs::Convert(_poseMsg), pose3Eql) ? ComponentState::OneTimeChange : ComponentState::NoChange; - this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, + _iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, state); } + return true; +} + +////////////////////////////////////////////////// +PoseCommand::PoseCommand(msgs::Pose *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PoseCommand::Execute() +{ + auto poseMsg = dynamic_cast(this->msg); + if (nullptr == poseMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + return updatePose(*poseMsg, this->iface); +} + +////////////////////////////////////////////////// +PoseVectorCommand::PoseVectorCommand(msgs::Pose_V *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PoseVectorCommand::Execute() +{ + auto poseVectorMsg = dynamic_cast(this->msg); + if (nullptr == poseVectorMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + for (int i = 0; i < poseVectorMsg->pose_size(); i++) + { + if (!updatePose(poseVectorMsg->pose(i), this->iface)) + { + return false; + } + } return true; } @@ -1616,6 +1837,130 @@ bool VisualCommand::Execute() return true; } +////////////////////////////////////////////////// +WheelSlipCommand::WheelSlipCommand(msgs::WheelSlipParametersCmd *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +// TODO(ivanpauno): Move this somewhere else +Entity scopedEntityFromMsg( + const msgs::Entity & _msg, const EntityComponentManager & _ecm) +{ + if (_msg.id() != kNullEntity) { + return _msg.id(); + } + std::unordered_set entities = entitiesFromScopedName( + _msg.name(), _ecm); + if (entities.empty()) { + ignerr << "Failed to find entity with scoped name [" << _msg.name() + << "]." << std::endl; + return kNullEntity; + } + if (_msg.type() == msgs::Entity::NONE) { + return *entities.begin(); + } + const components::BaseComponent * component; + std::string componentType; + for (const auto entity : entities) { + switch (_msg.type()) { + case msgs::Entity::LIGHT: + component = _ecm.Component(entity); + componentType = "LIGHT"; + break; + case msgs::Entity::MODEL: + component = _ecm.Component(entity); + componentType = "MODEL"; + break; + case msgs::Entity::LINK: + component = _ecm.Component(entity); + componentType = "LINK"; + break; + case msgs::Entity::VISUAL: + component = _ecm.Component(entity); + componentType = "VISUAL"; + break; + case msgs::Entity::COLLISION: + component = _ecm.Component(entity); + componentType = "COLLISION"; + break; + case msgs::Entity::SENSOR: + component = _ecm.Component(entity); + componentType = "SENSOR"; + break; + case msgs::Entity::JOINT: + component = _ecm.Component(entity); + componentType = "JOINT"; + break; + default: + componentType = "unknown"; + break; + } + if (component != nullptr) { + return entity; + } + } + ignerr << "Found entity with scoped name [" << _msg.name() + << "], but it doesn't have a component of the required type [" + << componentType << "]." << std::endl; + return kNullEntity; +} + +////////////////////////////////////////////////// +bool WheelSlipCommand::Execute() +{ + auto wheelSlipMsg = dynamic_cast( + this->msg); + if (nullptr == wheelSlipMsg) + { + ignerr << "Internal error, null wheel slip message" << std::endl; + return false; + } + const auto & ecm = *this->iface->ecm; + Entity entity = scopedEntityFromMsg(wheelSlipMsg->entity(), ecm); + if (kNullEntity == entity) + { + return false; + } + + auto doForEachLink = [this, wheelSlipMsg](Entity linkEntity) { + auto wheelSlipCmdComp = + this->iface->ecm->Component(linkEntity); + if (!wheelSlipCmdComp) + { + this->iface->ecm->CreateComponent( + linkEntity, components::WheelSlipCmd(*wheelSlipMsg)); + } + else + { + auto state = wheelSlipCmdComp->SetData( + *wheelSlipMsg, this->wheelSlipEql) ? ComponentState::OneTimeChange + : ComponentState::NoChange; + this->iface->ecm->SetChanged( + linkEntity, components::WheelSlipCmd::typeId, state); + } + }; + const components::BaseComponent * component = + ecm.Component(entity); + + if (nullptr != component) { + doForEachLink(entity); + return true; + } + component = ecm.Component(entity); + if (nullptr != component) { + Model model{entity}; + for (const auto & linkEntity : model.Links(*this->iface->ecm)) { + doForEachLink(linkEntity); + } + return true; + } + ignerr << "Found entity with scoped name [" << wheelSlipMsg->entity().name() + << "], is neither a model or a link." << std::endl; + return false; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index 4b0870367b..ea162bd078 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/src/systems/user_commands/UserCommands.hh @@ -53,6 +53,22 @@ namespace systems /// * **Request type*: ignition.msgs.EntityFactory_V /// * **Response type*: ignition.msgs.Boolean /// + /// # Set entity pose + /// + /// This service set the pose of entities + /// + /// * **Service**: `/world//set_pose` + /// * **Request type*: ignition.msgs.Pose + /// * **Response type*: ignition.msgs.Boolean + /// + /// # Set multiple entity poses + /// + /// This service set the pose of multiple entities + /// + /// * **Service**: `/world//set_pose_vector` + /// * **Request type*: ignition.msgs.Pose_V + /// * **Response type*: ignition.msgs.Boolean + /// /// Try some examples described on examples/worlds/empty.sdf class UserCommands final: public System, diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index df1dd2c5dd..78385295de 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" using namespace ignition; using namespace gazebo; @@ -107,7 +108,9 @@ class ignition::gazebo::systems::WheelSlipPrivate { if (_a.size() != _b.size() || _a.size() < 2 ||_b.size() < 2) + { return false; + } for (size_t i = 0; i < _a.size(); i++) { @@ -242,9 +245,32 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, ///////////////////////////////////////////////// void WheelSlipPrivate::Update(EntityComponentManager &_ecm) { - for (const auto &linkSurface : this->mapLinkSurfaceParams) + for (auto &linkSurface : this->mapLinkSurfaceParams) { - const auto ¶ms = linkSurface.second; + auto ¶ms = linkSurface.second; + const auto * wheelSlipCmdComp = + _ecm.Component(linkSurface.first); + if (wheelSlipCmdComp) + { + const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data(); + bool changed = (!math::equal( + params.slipComplianceLateral, + wheelSlipCmdParams.slip_compliance_lateral(), + 1e-6)) || + (!math::equal( + params.slipComplianceLongitudinal, + wheelSlipCmdParams.slip_compliance_longitudinal(), + 1e-6)); + + if (changed) + { + params.slipComplianceLateral = + wheelSlipCmdParams.slip_compliance_lateral(); + params.slipComplianceLongitudinal = + wheelSlipCmdParams.slip_compliance_longitudinal(); + } + _ecm.RemoveComponent(linkSurface.first); + } // get user-defined normal force constant double force = params.wheelNormalForce; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 5b9dc95c13..7d38a0f889 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -36,6 +36,8 @@ set(tests logical_audio_sensor_plugin.cc magnetometer_system.cc model.cc + model_photo_shoot_default_joints.cc + model_photo_shoot_random_joints.cc multicopter.cc multiple_servers.cc navsat_system.cc @@ -76,10 +78,12 @@ set(tests_needing_display camera_sensor_background.cc camera_video_record_system.cc depth_camera.cc + distortion_camera.cc gpu_lidar.cc optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc + sensors_system_battery.cc shader_param_system.cc thermal_system.cc thermal_sensor_system.cc @@ -133,9 +137,23 @@ target_link_libraries(INTEGRATION_tracked_vehicle_system ignition-physics${IGN_PHYSICS_VER}::core ignition-plugin${IGN_PLUGIN_VER}::loader ) + +target_link_libraries(INTEGRATION_model_photo_shoot_default_joints + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) +target_link_libraries(INTEGRATION_model_photo_shoot_random_joints + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) + # The default timeout (240s) doesn't seem to be enough for this test. set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) if(TARGET INTEGRATION_examples_build) set_tests_properties(INTEGRATION_examples_build PROPERTIES TIMEOUT 320) endif() + +if(VALID_DISPLAY AND VALID_DRI_DISPLAY) + target_link_libraries(INTEGRATION_sensors_system + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ) +endif() diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh new file mode 100644 index 0000000000..68e0923646 --- /dev/null +++ b/test/integration/ModelPhotoShootTest.hh @@ -0,0 +1,306 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the \"License\"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an \"AS IS\" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ +#define IGNITION_GAZEBO_TEST_INTEGRATION_MODELPHOTOSHOOTTEST_HH_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/Model.hh" + +#include +#include +#include +#include +#include +#include + + +#include "helpers/UniqueTestDirectoryEnv.hh" +#include "helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Saves an image from a camera in a given position. +/// \param[in] _camera Camera to use for the picture. +/// \param[in] _pose Pose for the camera. +/// \param[in] _fileName Filename to save the image. +void SavePicture(const ignition::rendering::CameraPtr _camera, + const ignition::math::Pose3d &_pose, + const std::string &_fileName) +{ + unsigned int width = _camera->ImageWidth(); + unsigned int height = _camera->ImageHeight(); + ignition::common::Image image; + + _camera->SetWorldPose(_pose); + auto cameraImage = _camera->CreateImage(); + _camera->Capture(cameraImage); + auto formatStr = + ignition::rendering::PixelUtil::Name(_camera->ImageFormat()); + auto format = ignition::common::Image::ConvertPixelFormat(formatStr); + image.SetFromData(cameraImage.Data(), width, height, format); + image.SavePNG(_fileName); +} + +/// \brief Tests that two png files have the same values. +/// \param[in] _filename First png file. +/// \param[in] _testFilename Second png file. +void testImages(const std::string &_imageFile, + const std::string &_testImageFile) +{ + std::string imageFilePath = common::joinPaths(common::cwd(), _imageFile); + ignition::common::Image image(imageFilePath); + std::string testImageFilePath = + common::joinPaths(common::cwd(), _testImageFile); + ignition::common::Image testImage(testImageFilePath); + + EXPECT_TRUE(image.Valid()); + EXPECT_TRUE(testImage.Valid()); + EXPECT_EQ(image.Width(), testImage.Width()); + EXPECT_EQ(image.Height(), testImage.Height()); + EXPECT_EQ(image.PixelFormat(), testImage.PixelFormat()); + unsigned int dataLength; + unsigned char *imageData = nullptr; + image.Data(&imageData, dataLength); + unsigned int testDataLenght; + unsigned char *testImageData = nullptr; + image.Data(&testImageData, testDataLenght); + ASSERT_EQ(dataLength, testDataLenght); + ASSERT_EQ(memcmp(imageData, testImageData, dataLength), 0); + + // Deleting files so they do not affect future tests + EXPECT_EQ(remove(imageFilePath.c_str()), 0); + EXPECT_EQ(remove(testImageFilePath.c_str()), 0); +} + +/// \brief Test ModelPhotoShootTest system. +class ModelPhotoShootTest : public InternalFixture<::testing::Test> +{ + protected: void SetUp() override + { + EXPECT_TRUE(common::chdir(test::UniqueTestDirectoryEnv::Path())); + InternalFixture<::testing::Test>::SetUp(); + } + /// \brief PostRender callback. + public: void OnPostRender() + { + if (takeTestPics) + { + ignition::rendering::ScenePtr scene = + ignition::rendering::sceneFromFirstRenderEngine(); + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto camera = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (nullptr != camera && camera->Name() == "photo_shoot::link::camera") + { + ignition::math::Pose3d pose; + // Perspective view + pose.Pos().Set(1.6 / scaling + translation.X(), + -1.6 / scaling + translation.Y(), + 1.2 / scaling + translation.Z()); + pose.Rot().SetFromEuler(0, IGN_DTOR(30), IGN_DTOR(-225)); + SavePicture(camera, pose, "1_test.png"); + // Top view + pose.Pos().Set(0 + translation.X(), + 0 + translation.Y(), + 2.2 / scaling + translation.Z()); + pose.Rot().SetFromEuler(0, IGN_DTOR(90), 0); + SavePicture(camera, pose, "2_test.png"); + + // Front view + pose.Pos().Set(2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, IGN_DTOR(-180)); + SavePicture(camera, pose, "3_test.png"); + + // Side view + pose.Pos().Set(0 + translation.X(), + 2.2 / scaling + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, IGN_DTOR(-90)); + SavePicture(camera, pose, "4_test.png"); + + // Back view + pose.Pos().Set(-2.2 / scaling + translation.X(), + 0 + translation.Y(), + 0 + translation.Z()); + pose.Rot().SetFromEuler(0, 0, 0); + SavePicture(camera, pose, "5_test.png"); + } + } + takeTestPics = false; + } + } + + /// \brief Loads the pose values generated by the Model Photo Shoot plugin. + /// \param[in] _poseFile File containing the generated poses. + protected: void LoadPoseValues(std::string _poseFile = "poses.txt") + { + std::string poseFilePath = common::joinPaths(common::cwd(), _poseFile); + std::ifstream poseFile (poseFilePath); + std::string line; + ASSERT_TRUE(poseFile.is_open()); + while (getline(poseFile, line) ) + { + std::istringstream iss(line); + std::string word; + while (getline( iss, word, ' ' )) + { + if (word == "Translation:") + { + float tr_x, tr_y, tr_z; + getline( iss, word, ' ' ); + tr_x = std::stof(word); + getline( iss, word, ' ' ); + tr_y = std::stof(word); + getline( iss, word, ' ' ); + tr_z = std::stof(word); + this->translation = {tr_x, tr_y, tr_z}; + break; + } + else + { + if (word == "Scaling:") + { + getline( iss, word, ' ' ); + this->scaling = std::stod(word); + break; + } + else + { + std::string jointName = line.substr(0, line.find(": ")); + std::string jointPose = line.substr(line.find(": ")+2); + jointPositions[jointName] = std::stod(jointPose); + } + } + } + } + poseFile.close(); + EXPECT_EQ(remove(poseFilePath.c_str()), 0); + } + + /// \brief Tests the Model Photo Shoot plugin with a given sdf world. + /// \param[in] _sdfWorld SDF World to use for the test. + protected: void ModelPhotoShootTestCmd(const std::string _sdfWorld) + { + // First run of the server generating images through the plugin. + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + _sdfWorld)); + fixture.Server()->SetUpdatePeriod(1ns); + + common::ConnectionPtr postRenderConn; + fixture.OnConfigure([&]( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &, + EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&ModelPhotoShootTest::OnPostRender, this)); + }).Finalize(); + + fixture.Server()->Run(true, 50, false); + this->LoadPoseValues(); + + fixture.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if(!jointPositions.empty() && this->checkRandomJoints) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + auto modelName = _ecm.Component(_entity); + if (modelName->Data() == "r2") + { + this->model = std::make_shared(_entity); + } + return true; + }); + std::vector joints = this->model->Joints(_ecm); + for (const auto &joint : joints) + { + auto jointNameComp = _ecm.Component(joint); + std::map::iterator it = + jointPositions.find(jointNameComp->Data()); + if(it != jointPositions.end()) + { + auto jointType = _ecm.Component + (joint)->Data(); + ASSERT_TRUE(jointType == sdf::JointType::REVOLUTE || + jointType == sdf::JointType::PRISMATIC); + auto jointAxis = _ecm.Component(joint); + ASSERT_GE(it->second, jointAxis->Data().Lower()); + ASSERT_LE(it->second, jointAxis->Data().Upper()); + auto jointPosition = + _ecm.Component(joint); + ASSERT_NE(jointPosition, nullptr); + ASSERT_DOUBLE_EQ(jointPosition->Data()[0], it->second); + } + } + this->checkRandomJoints = false; + } + }).Finalize(); + + this->takeTestPics = true; + + const auto end_time = std::chrono::steady_clock::now() + + std::chrono::milliseconds(3000); + while (takeTestPics && end_time > std::chrono::steady_clock::now()) + { + fixture.Server()->Run(true, 1, false); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + testImages("1.png", "1_test.png"); + testImages("2.png", "2_test.png"); + testImages("3.png", "3_test.png"); + testImages("4.png", "4_test.png"); + testImages("5.png", "5_test.png"); + } + + private: bool takeTestPics{false}; + private: bool checkRandomJoints{true}; + private: double scaling; + private: ignition::math::Vector3d translation; + private: std::map jointPositions; + private: std::shared_ptr model; +}; + +#endif diff --git a/test/integration/distortion_camera.cc b/test/integration/distortion_camera.cc new file mode 100644 index 0000000000..279e6d28e7 --- /dev/null +++ b/test/integration/distortion_camera.cc @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2022 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 + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" +#include +#include +#include +#include +#include + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test WideAndleCameraTest system +class DistortionCameraTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + mutex.lock(); + unsigned int imageSamples = _msg.width() * _msg.height() * 3; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples]; + memcpy(imageBuffer, _msg.data().c_str(), imageSamples); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the Distortion Camera readings +TEST_F(DistortionCameraTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(DistortionCameraBox)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "camera_distortion.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Subscribe to the image topic + transport::Node node; + node.Subscribe("/camera_sensor_barrel", &imageCb); + + // Run server and verify that we are receiving a message + // from the distortion camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + delete[] imageBuffer; +} diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 1068c21174..7e6f4e4afa 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -178,10 +178,17 @@ TEST_F(JointPositionControllerTestFixture, server.AddSystem(testSystem.systemPtr); - const std::size_t initIters = 10; + // joint pos starts at 0 + const std::size_t initIters = 1; server.Run(true, initIters, false); EXPECT_NEAR(0, currentPosition.at(0), TOL); + // joint moves to initial_position at -2.0 + const std::size_t initPosIters = 1000; + server.Run(true, initPosIters, false); + double expectedInitialPosition = -2.0; + EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL); + // Publish command and check that the joint position is set transport::Node node; auto pub = node.Advertise( diff --git a/test/integration/joint_state_publisher_system.cc b/test/integration/joint_state_publisher_system.cc index d5cb4a090e..6c0798ef37 100644 --- a/test/integration/joint_state_publisher_system.cc +++ b/test/integration/joint_state_publisher_system.cc @@ -144,3 +144,57 @@ TEST_F(JointStatePublisherTest, // Make sure the callback was triggered at least once. EXPECT_GT(count, 0); } + +///////////////////////////////////////////////// +TEST_F(JointStatePublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(NestedJointPublisher)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "diff_drive_nested.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + int count = 0; + // Check that all of joints are published. + std::function jointStateCb = + [&](const msgs::Model &_msg) + { + bool foundLeftWheelJoint{false}, + foundRightWheelJoint{false}, + foundCasterWheel{false}, + extra{false}; + + for (int i = 0; i < _msg.joint_size(); ++i) + { + if (_msg.joint(i).name() == "left_wheel_joint") + foundLeftWheelJoint = true; + else if (_msg.joint(i).name() == "right_wheel_joint") + foundRightWheelJoint = true; + else if (_msg.joint(i).name() == "caster_wheel") + foundCasterWheel = true; + else + extra = true; + } + EXPECT_TRUE(foundLeftWheelJoint); + EXPECT_TRUE(foundRightWheelJoint); + EXPECT_TRUE(foundCasterWheel); + EXPECT_FALSE(extra); + count++; + }; + + transport::Node node; + node.Subscribe( + "/world/diff_drive_nested/model/vehicle/model/vehicle_nested/joint_state", + jointStateCb); + + server.Run(true, 10, false); + + // Make sure the callback was triggered at least once. + EXPECT_GT(count, 0); +} diff --git a/test/integration/model_photo_shoot_default_joints.cc b/test/integration/model_photo_shoot_default_joints.cc new file mode 100644 index 0000000000..620bf42e8a --- /dev/null +++ b/test/integration/model_photo_shoot_default_joints.cc @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2022 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 "ModelPhotoShootTest.hh" + +#include + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootDefaultJoints)) +{ + this->ModelPhotoShootTestCmd( + "examples/worlds/model_photo_shoot.sdf"); +} + +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("model_photo_shoot_test")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/model_photo_shoot_random_joints.cc b/test/integration/model_photo_shoot_random_joints.cc new file mode 100644 index 0000000000..21cabd9681 --- /dev/null +++ b/test/integration/model_photo_shoot_random_joints.cc @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2022 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 "ModelPhotoShootTest.hh" + +#include + +// Test the Model Photo Shoot plugin on the example world. +TEST_F(ModelPhotoShootTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelPhotoShootRandomJoints)) +{ + this->ModelPhotoShootTestCmd( + "test/worlds/model_photo_shoot_random_joints.sdf"); +} + +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("model_photo_shoot_test")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index c60e19588e..d5994ac36c 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -394,6 +394,157 @@ class OdometryPublisherTest EXPECT_EQ(5u, odomPosesCount); } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestOffsetTags(const std::string &_sdfFile, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + std::vector odomPoses; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Run for 3s and check the pose in the last message + ASSERT_FALSE(odomPoses.empty()); + auto lastPose = odomPoses[odomPoses.size() - 1]; + EXPECT_NEAR(lastPose.Pos().X(), 11, 1e-2); + EXPECT_NEAR(lastPose.Pos().Y(), -11, 1e-2); + EXPECT_NEAR(lastPose.Pos().Z(), 0, 1e-2); + + EXPECT_NEAR(lastPose.Rot().Roll(), 1.57, 1e-2); + EXPECT_NEAR(lastPose.Rot().Pitch(), 0, 1e-2); + EXPECT_NEAR(lastPose.Rot().Yaw(), 0, 1e-2); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestGaussianNoise(const std::string &_sdfFile, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + std::vector odomLinVels; + std::vector odomAngVels; + google::protobuf::RepeatedField odomTwistCovariance; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::OdometryWithCovariance &_msg) + { + odomLinVels.push_back(msgs::Convert(_msg.twist_with_covariance(). + twist().linear())); + odomAngVels.push_back(msgs::Convert(_msg.twist_with_covariance(). + twist().angular())); + odomTwistCovariance = _msg.twist_with_covariance().covariance().data(); + }; + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + int sleep = 0; + int maxSleep = 30; + for (; odomLinVels.size() < 500 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Verify the Gaussian noise. + ASSERT_FALSE(odomLinVels.empty()); + ASSERT_FALSE(odomAngVels.empty()); + int n = odomLinVels.size(); + + // Calculate the means. + double linVelSumX = 0, linVelSumY = 0, linVelSumZ = 0; + double angVelSumX = 0, angVelSumY = 0, angVelSumZ = 0; + for (int i = 0; i < n; i++) + { + linVelSumX += odomLinVels[i].X(); + linVelSumY += odomLinVels[i].Y(); + linVelSumZ += odomLinVels[i].Z(); + + angVelSumX += odomAngVels[i].X(); + angVelSumY += odomAngVels[i].Y(); + angVelSumZ += odomAngVels[i].Z(); + } + + // Check that the mean values are close to zero. + EXPECT_NEAR(linVelSumX/n, 0, 0.3); + EXPECT_NEAR(linVelSumY/n, 0, 0.3); + EXPECT_NEAR(linVelSumZ/n, 0, 0.3); + + EXPECT_NEAR(angVelSumX/n, 0, 0.3); + EXPECT_NEAR(angVelSumY/n, 0, 0.3); + EXPECT_NEAR(angVelSumZ/n, 0, 0.3); + + // Calculate the variation (sigma^2). + double linVelSqSumX = 0, linVelSqSumY = 0, linVelSqSumZ = 0; + double angVelSqSumX = 0, angVelSqSumY = 0, angVelSqSumZ = 0; + for (int i = 0; i < n; i++) + { + linVelSqSumX += std::pow(odomLinVels[i].X() - linVelSumX/n, 2); + linVelSqSumY += std::pow(odomLinVels[i].Y() - linVelSumY/n, 2); + linVelSqSumZ += std::pow(odomLinVels[i].Z() - linVelSumZ/n, 2); + + angVelSqSumX += std::pow(odomAngVels[i].X() - angVelSumX/n, 2); + angVelSqSumY += std::pow(odomAngVels[i].Y() - angVelSumY/n, 2); + angVelSqSumZ += std::pow(odomAngVels[i].Z() - angVelSumZ/n, 2); + } + + // Verify the variance values. + EXPECT_NEAR(linVelSqSumX/n, 1, 0.3); + EXPECT_NEAR(linVelSqSumY/n, 1, 0.3); + EXPECT_NEAR(linVelSqSumZ/n, 1, 0.3); + + EXPECT_NEAR(angVelSqSumX/n, 1, 0.3); + EXPECT_NEAR(angVelSqSumY/n, 1, 0.3); + EXPECT_NEAR(angVelSqSumZ/n, 1, 0.3); + + // Check the covariance matrix. + EXPECT_EQ(odomTwistCovariance.size(), 36); + for (int i = 0; i < 36; i++) + { + if (i % 7 == 0) + { + EXPECT_NEAR(odomTwistCovariance.Get(i), 1, 1e-2); + } + else + { + EXPECT_NEAR(odomTwistCovariance.Get(i), 0, 1e-2); + } + } + } }; ///////////////////////////////////////////////// @@ -446,6 +597,26 @@ TEST_P(OdometryPublisherTest, "baseCustom"); } +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetTagTest)) +{ + TestOffsetTags( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_offset.sdf", + "/model/vehicle/odometry"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(GaussianNoiseTest)) +{ + TestGaussianNoise( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_noise.sdf", + "/model/vehicle/odometry_with_covariance"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest, ::testing::Range(1, 2)); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 9775f23864..b62b4d1995 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -620,13 +620,15 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(StateStatic)) } ///////////////////////////////////////////////// -/// Test whether the scene topic is published when a component is removed. -TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) +/// Test whether the scene topic is published when entities and components are +/// removed/added +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(AddRemoveEntitiesComponents)) { // Start server ignition::gazebo::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/shapes.sdf"); + "/test/worlds/shapes_scene_broadcaster_only.sdf"); gazebo::Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -638,7 +640,10 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) testSystem.OnUpdate([](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) { - if (_info.iterations > 1) + static bool periodicChangeMade = false; + + // remove a component from an entity + if (_info.iterations == 2) { _ecm.Each(boxEntity, + ignition::gazebo::components::Pose({1, 2, 3, 4, 5, 6})); + EXPECT_TRUE(_ecm.EntityHasComponentType(boxEntity, + ignition::gazebo::components::Pose::typeId)); + } + // remove an entity + else if (_info.iterations == 4) + { + auto boxEntity = _ecm.EntityByComponents( + gazebo::components::Name("box"), gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, boxEntity); + _ecm.RequestRemoveEntity(boxEntity); + } + // create an entity + else if (_info.iterations == 5) + { + EXPECT_EQ(gazebo::kNullEntity, _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model())); + auto newEntity = _ecm.CreateEntity(); + _ecm.CreateComponent(newEntity, gazebo::components::Name("newEntity")); + _ecm.CreateComponent(newEntity, gazebo::components::Model()); + EXPECT_NE(gazebo::kNullEntity, _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model())); + } + // modify an existing component via OneTimeChange + else if (_info.iterations == 6) + { + auto entity = _ecm.EntityByComponents( + gazebo::components::Name("newEntity"), + gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, entity); + EXPECT_TRUE(_ecm.SetComponentData(entity, + "newEntity1")); + _ecm.SetChanged(entity, gazebo::components::Name::typeId, + gazebo::ComponentState::OneTimeChange); + } + // modify an existing component via PeriodicChange + else if (_info.iterations > 6 && !periodicChangeMade) + { + auto entity = _ecm.EntityByComponents( + gazebo::components::Name("newEntity1"), + gazebo::components::Model()); + ASSERT_NE(gazebo::kNullEntity, entity); + EXPECT_TRUE(_ecm.SetComponentData(entity, + "newEntity2")); + _ecm.SetChanged(entity, gazebo::components::Name::typeId, + gazebo::ComponentState::PeriodicChange); + periodicChangeMade = true; + } }); server.AddSystem(testSystem.systemPtr); + int receivedStates = 0; bool received = false; bool hasState = false; + ignition::gazebo::EntityComponentManager localEcm; std::function cb = [&](const msgs::SerializedStepMap &_res) { + hasState = _res.has_state(); // Check the received state. if (hasState) { - ignition::gazebo::EntityComponentManager localEcm; + receivedStates++; + localEcm.SetState(_res.state()); bool hasBox = false; + bool hasNewEntity = false; + bool hasModifiedComponent = false; + bool newEntityIteration = _res.stats().iterations() == 5; + bool oneTimeChangeIteration = _res.stats().iterations() == 6; + bool periodicChangeIteration = _res.stats().iterations() > 7; localEcm.Each( [&](const ignition::gazebo::Entity &_entity, @@ -679,22 +752,52 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) if (_name->Data() == "box") { hasBox = true; - if (_res.stats().iterations() > 1) + if (_res.stats().iterations() != 2) { - // The pose component should be gone - EXPECT_FALSE(localEcm.EntityHasComponentType( + // The pose component should exist + EXPECT_TRUE(localEcm.EntityHasComponentType( _entity, ignition::gazebo::components::Pose::typeId)); } else { - // The pose component should exist - EXPECT_TRUE(localEcm.EntityHasComponentType( + // The pose component should be gone + EXPECT_FALSE(localEcm.EntityHasComponentType( _entity, ignition::gazebo::components::Pose::typeId)); } } + + if (newEntityIteration && _name->Data() == "newEntity") + hasNewEntity = true; + + if (oneTimeChangeIteration && _name->Data() == "newEntity1") + hasModifiedComponent = true; + else if (periodicChangeIteration && _name->Data() == "newEntity2") + hasModifiedComponent = true; + return true; }); + + // make sure that the box entity is marked as removed + if (_res.stats().iterations() >= 4) + { + bool markedAsRemoved = false; + localEcm.EachRemoved( + [&](const ignition::gazebo::Entity &, + const ignition::gazebo::components::Model *, + const ignition::gazebo::components::Name *_name)->bool + { + if (_name->Data() == "box") + markedAsRemoved = true; + return true; + }); + EXPECT_TRUE(markedAsRemoved); + } + EXPECT_TRUE(hasBox); + EXPECT_EQ(newEntityIteration, hasNewEntity); + EXPECT_EQ(periodicChangeIteration || oneTimeChangeIteration, + hasModifiedComponent); } received = true; }; @@ -702,43 +805,73 @@ TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RemovedComponent)) transport::Node node; EXPECT_TRUE(node.Subscribe("/world/default/state", cb)); - unsigned int sleep = 0u; - unsigned int maxSleep = 30u; + // Helper method that runs the server one iteration and then checks that + // received data was processed correctly. + // The _shouldHaveState parameter defines whether the published + // msgs::SerializedStepMap should contain state info or not + std::function runServerOnce = + [&](bool _shouldHaveState) + { + unsigned int sleep = 0u; + unsigned int maxSleep = 30u; + received = false; + hasState = false; + + server.RunOnce(true); + // cppcheck-suppress unmatchedSuppression + // cppcheck-suppress knownConditionTrueFalse + while (!received && sleep++ < maxSleep) + IGN_SLEEP_MS(100); + EXPECT_TRUE(received); + EXPECT_EQ(_shouldHaveState, hasState); + }; // Run server once. The first time should send the state message - server.RunOnce(true); - // cppcheck-suppress unmatchedSuppression - // cppcheck-suppress knownConditionTrueFalse - while (!received && sleep++ < maxSleep) - IGN_SLEEP_MS(100); - EXPECT_TRUE(received); - EXPECT_TRUE(hasState); + runServerOnce(true); // Run server again. The second time shouldn't have state info. The // message can still arrive due the passage of time (see `itsPubTime` in // SceneBroadcaster::PostUpdate. - sleep = 0u; - received = false; - hasState = false; - server.RunOnce(true); - // cppcheck-suppress unmatchedSuppression - // cppcheck-suppress knownConditionTrueFalse - while (!received && sleep++ < maxSleep) - IGN_SLEEP_MS(100); - EXPECT_FALSE(hasState); + runServerOnce(false); // Run server again. The third time should send the state message because // the test system removed a component. - sleep = 0u; + runServerOnce(true); + + // Run server again. The fourth time should send the state message because + // the test system added a component. + runServerOnce(true); + + // Run server again. The fifth time should send the state message because + // the test system requested to remove an entity. + runServerOnce(true); + + // Run server again. The sixth time should send the state message because + // the test system created an entity. + runServerOnce(true); + + // Run server again. The seventh time should send the state message because + // the test system modified a component and marked it as a OneTimeChange. + runServerOnce(true); + + // Run server for a few iterations to make sure that the periodic change + // made by the test system is received. received = false; hasState = false; - server.RunOnce(true); + server.Run(true, 10, false); + // (wait for a bit after running the server in case ign-transport is still + // processing messages) + unsigned int sleep = 0u; + unsigned int maxSleep = 50u; // cppcheck-suppress unmatchedSuppression // cppcheck-suppress knownConditionTrueFalse while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); EXPECT_TRUE(received); EXPECT_TRUE(hasState); + + // Sanity check: make sure that at least 7 states were received and processed + EXPECT_GE(receivedStates, 7); } // Run multiple times diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 6165ee9620..f640add611 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -27,6 +28,11 @@ #include #include +#include +#include +#include +#include + #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -37,8 +43,11 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/rendering/Events.hh" + #include "plugins/MockSystem.hh" #include "../helpers/EnvTestFixture.hh" @@ -46,6 +55,45 @@ using namespace ignition; using namespace std::chrono_literals; namespace components = ignition::gazebo::components; +std::unordered_set g_sensorEntityIds; +rendering::ScenePtr g_scene; + +///////////////////////////////////////////////// +void OnPostRender() +{ + if (!g_scene) + { + g_scene = rendering::sceneFromFirstRenderEngine(); + } + ASSERT_TRUE(g_scene); + + EXPECT_LT(0u, g_scene->SensorCount()); + for (unsigned int i = 0; i < g_scene->SensorCount(); ++i) + { + auto sensor = g_scene->SensorByIndex(i); + ASSERT_TRUE(sensor); + EXPECT_TRUE(sensor->HasUserData("gazebo-entity")); + auto variant = sensor->UserData("gazebo-entity"); + + // todo(anyone) change int to uint64_t once user data supports it + const int *value = std::get_if(&variant); + ASSERT_TRUE(value); + g_sensorEntityIds.insert(*value); + } +} + +///////////////////////////////////////////////// +void testSensorEntityIds(const gazebo::EntityComponentManager &_ecm, + const std::unordered_set &_ids) +{ + EXPECT_FALSE(_ids.empty()); + for (const auto & id : _ids) + { + auto sensorComp = _ecm.Component(id); + EXPECT_TRUE(sensorComp); + } +} + ////////////////////////////////////////////////// class SensorsFixture : public InternalFixture> { @@ -127,6 +175,8 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) gazebo::Server server(serverConfig); + common::ConnectionPtr postRenderConn; + // A pointer to the ecm. This will be valid once we run the mock system gazebo::EntityComponentManager *ecm = nullptr; this->mockSystem->preUpdateCallback = @@ -134,6 +184,15 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) { ecm = &_ecm; }; + this->mockSystem->configureCallback = + [&](const gazebo::Entity &, + const std::shared_ptr &, + gazebo::EntityComponentManager &, + gazebo::EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&::OnPostRender)); + }; server.AddSystem(this->systemPtr); server.Run(true, 50, false); @@ -141,6 +200,10 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) testDefaultTopics(); + testSensorEntityIds(*ecm, g_sensorEntityIds); + g_sensorEntityIds.clear(); + g_scene.reset(); + // We won't use the event manager but it's required to create an // SdfEntityCreator gazebo::EventManager dummyEventMgr; diff --git a/test/integration/sensors_system_battery.cc b/test/integration/sensors_system_battery.cc new file mode 100644 index 0000000000..541ae9ac05 --- /dev/null +++ b/test/integration/sensors_system_battery.cc @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2022 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 "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/test_config.hh" + +#include "ignition/gazebo/components/BatterySoC.hh" +#include "ignition/gazebo/components/Name.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +namespace components = ignition::gazebo::components; + +unsigned int imgCount = 0u; +unsigned int depthImgCount = 0u; +std::mutex mutex; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &) +{ + std::lock_guard lock(mutex); + imgCount++; +} + +///////////////////////////////////////////////// +void depthCb(const msgs::Image &) +{ + std::lock_guard lock(mutex); + depthImgCount++; +} + +////////////////////////////////////////////////// +class SensorsFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + auto plugin = sm.LoadPlugin("libMockSystem.so", + "ignition::gazebo::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: ignition::gazebo::SystemPluginPtr systemPtr; + public: gazebo::MockSystem *mockSystem; + + private: gazebo::SystemLoader sm; +}; + +///////////////////////////////////////////////// +// Battery +TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(SensorsBatteryState)) +{ + const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "sensors_system_battery.sdf"); + sdf::Root root; + EXPECT_EQ(root.Load(sdfPath).size(), 0lu); + EXPECT_GT(root.WorldCount(), 0lu); + + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfPath); + + // A pointer to the ecm. This will be valid once we run the mock system + gazebo::EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }; + + // Start server + Server server(serverConfig); + server.AddSystem(this->systemPtr); + server.Run(true, 100, false); + EXPECT_NE(nullptr, ecm); + + // Check a battery exists + EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId)); + + // Find the battery entity + Entity batEntity = ecm->EntityByComponents(components::Name( + "linear_battery")); + EXPECT_NE(kNullEntity, batEntity); + + // Find the battery component + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity, + components::BatterySoC::typeId)); + auto batComp = ecm->Component(batEntity); + + // Check state of charge should be 1, since the batery has not drained + // and the is equivalent ot the . + EXPECT_DOUBLE_EQ(batComp->Data(), 1.0); + + ignition::transport::Node node; + + // subscribe to img topics to make sure we are receiving images. + node.Subscribe("/camera", &imageCb); + node.Subscribe("/depth_camera", &depthCb); + + // Send a message on one of the topics, which will + // start the battery draining when the server starts again. + auto pub = node.Advertise("/battery/discharge"); + msgs::StringMsg msg; + pub.Publish(msg); + + // Run the server again. + server.Run(true, 50, false); + + // The state of charge should be <1, since the batery has started + // draining. + EXPECT_LT(batComp->Data(), 1.0); + + // wait and make sure camera is publishing images + int sleep = 0; + int maxSleep = 50; + unsigned int icount = 0u; + unsigned int dcount = 0u; + while ((icount <= 0u || dcount <= 0u) && sleep++ < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(mutex); + icount = imgCount; + dcount = depthImgCount; + } + { + std::lock_guard lock(mutex); + EXPECT_LT(0u, imgCount); + EXPECT_LT(0u, depthImgCount); + imgCount = 0u; + depthImgCount = 0u; + } + + unsigned int totalIter = 0u; + unsigned int iterToRun = 50u; + + // Run until battery is completely drained + while (batComp->Data() > 0.0 && totalIter < 1000) + { + server.Run(true, iterToRun, false); + totalIter += iterToRun; + } + EXPECT_LE(batComp->Data(), 0.0); + + // wait and make sure we receive expected no. of images + int rate = 30; + unsigned int expectedImgCount = totalIter / rate; + sleep = 0; + icount = 0u; + dcount = 0u; + while ((icount < expectedImgCount || dcount << expectedImgCount) + && sleep++ < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(mutex); + icount = imgCount; + dcount = depthImgCount; + } + + // verify image count + { + std::lock_guard lock(mutex); + EXPECT_EQ(expectedImgCount, imgCount); + EXPECT_EQ(expectedImgCount, depthImgCount); + imgCount = 0u; + depthImgCount = 0u; + } + + // sensors should be disabled. + // run for more iterations and verify we do not receive more images + for (int i = 0; i < 5; ++i) + { + server.Run(true, iterToRun, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(mutex); + EXPECT_EQ(0u, imgCount); + EXPECT_EQ(0u, depthImgCount); + } +} diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 18f4d4a432..14a7febd5d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -33,7 +33,9 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/WheelSlipCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -690,6 +692,81 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Pose)) EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PoseVector)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/shapes.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // Entity move by name + msgs::Pose_V req; + + auto poseBoxMsg = req.add_pose(); + poseBoxMsg->set_name("box"); + poseBoxMsg->mutable_position()->set_y(123.0); + + auto poseSphereMsg = req.add_pose(); + poseSphereMsg->set_name("sphere"); + poseSphereMsg->mutable_position()->set_y(456.0); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/default/set_pose_vector"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Box entity + auto boxEntity = ecm->EntityByComponents(components::Name("box")); + EXPECT_NE(kNullEntity, boxEntity); + + // Check entity has not been moved yet + auto poseComp = ecm->Component(boxEntity); + ASSERT_NE(nullptr, poseComp); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), poseComp->Data()); + + // Run an iteration and check it was moved + server.Run(true, 1, false); + + poseComp = ecm->Component(boxEntity); + ASSERT_NE(nullptr, poseComp); + EXPECT_NEAR(123.0, poseComp->Data().Pos().Y(), 0.2); + + auto sphereEntity = ecm->EntityByComponents(components::Name("sphere")); + EXPECT_NE(kNullEntity, sphereEntity); + + poseComp = ecm->Component(sphereEntity); + ASSERT_NE(nullptr, poseComp); + EXPECT_NEAR(456, poseComp->Data().Pos().Y(), 0.2); +} + ///////////////////////////////////////////////// // https://github.com/ignitionrobotics/ign-gazebo/issues/634 TEST_F(UserCommandsTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light)) @@ -758,6 +835,14 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light)) req.set_attenuation_constant(0.6f); req.set_attenuation_quadratic(0.001f); req.set_cast_shadows(true); + + // todo(ahcorde) Use the field is_light_off in light.proto from + // Garden on. + auto header = req.mutable_header()->add_data(); + header->set_key("isLightOn"); + std::string *value = header->add_value(); + *value = std::to_string(true); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); EXPECT_TRUE(result); EXPECT_TRUE(res.data()); @@ -1021,3 +1106,103 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Physics)) EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WheelSlip)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/trisphere_cycle_wheel_slip.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + ASSERT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + ASSERT_NE(kNullEntity, worldEntity); + Entity tc0 = ecm->EntityByComponents( + components::Name("trisphere_cycle0")); + ASSERT_NE(kNullEntity, tc0); + Entity tc1 = ecm->EntityByComponents( + components::Name("trisphere_cycle1")); + ASSERT_NE(kNullEntity, tc1); + + Model tcModel0{tc0}; + Model tcModel1{tc1}; + Entity wf0 = tcModel0.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf0); + Entity wrl0 = tcModel0.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl0); + Entity wrf0 = tcModel0.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf0); + Entity wf1 = tcModel1.LinkByName(*ecm, "wheel_front"); + ASSERT_NE(kNullEntity, wf1); + Entity wrl1 = tcModel1.LinkByName(*ecm, "wheel_rear_left"); + ASSERT_NE(kNullEntity, wrl1); + Entity wrf1 = tcModel1.LinkByName(*ecm, "wheel_rear_right"); + ASSERT_NE(kNullEntity, wrf1); + + Entity links[] = {wf0, wrl0, wrf0, wf1, wrl1, wrf1}; + for (auto link : links) { + EXPECT_EQ(nullptr, ecm->Component(link)); + } + + // modify wheel slip parameters of one link of model 0 + msgs::WheelSlipParametersCmd req; + auto * entityMsg = req.mutable_entity(); + entityMsg->set_name("trisphere_cycle0::wheel_front"); + entityMsg->set_type(msgs::Entity::LINK); + req.set_slip_compliance_lateral(1); + req.set_slip_compliance_longitudinal(1); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/wheel_slip/wheel_slip"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 2, false); + + // modify wheel slip parameters of one link of model 1 + entityMsg->set_name("trisphere_cycle1"); + entityMsg->set_type(msgs::Entity::MODEL); + req.set_slip_compliance_lateral(2); + req.set_slip_compliance_longitudinal(1); + + result = false; + res = msgs::Boolean{}; + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the WheelSlipCmd component is created + // and processed. + // The second one is just to check everything went fine. + server.Run(true, 3, false);} diff --git a/test/worlds/camera_distortion.sdf b/test/worlds/camera_distortion.sdf new file mode 100644 index 0000000000..dca73f8535 --- /dev/null +++ b/test/worlds/camera_distortion.sdf @@ -0,0 +1,168 @@ + + + + + 0.001 + 1.0 + + + + + + + ogre + 0 0 0 1 + + + + true + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + 0.0 0.0 -1.0 + + + + true + 3 0 -0.5 0 0 0 + + + + + 5 8 + + + + + + + 5 8 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + 1 + 30 + camera_sensor_undistorted + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + -0.1349 + -0.51868 + -0.001 + 0 + 0 +
0.5 0.5
+ + + 1 + 30 + camera_sensor_barrel + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + 0.1349 + 0.51868 + 0.001 + 0 + 0 +
0.5 0.5
+ + + 1 + 30 + camera_sensor_pincushion + + + + + + diff --git a/test/worlds/diff_drive_nested.sdf b/test/worlds/diff_drive_nested.sdf new file mode 100644 index 0000000000..1b7b914e34 --- /dev/null +++ b/test/worlds/diff_drive_nested.sdf @@ -0,0 +1,255 @@ + + + + + + 0.001 + 0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + -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 + + + + + + + + + + + 0.01 0.01 0.01 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + + + + + + + + diff --git a/test/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf index cd1c4e776c..8d382ad10b 100644 --- a/test/worlds/joint_position_controller_velocity.sdf +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -115,6 +115,7 @@ j1 true 1000 + -2.0 diff --git a/test/worlds/model_photo_shoot_random_joints.sdf b/test/worlds/model_photo_shoot_random_joints.sdf new file mode 100644 index 0000000000..dd22d60677 --- /dev/null +++ b/test/worlds/model_photo_shoot_random_joints.sdf @@ -0,0 +1,55 @@ + + + + + 0 0 0 + + + + ogre2 + 1, 1, 1 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + true + + + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + + + diff --git a/test/worlds/model_plugin_only.sdf b/test/worlds/model_plugin_only.sdf new file mode 100644 index 0000000000..6293cf5004 --- /dev/null +++ b/test/worlds/model_plugin_only.sdf @@ -0,0 +1,44 @@ + + + + + + + + + + + 1.14395 + + 0.126164 + 0.416519 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + + + + 2.01142 1 0.568726 + + + + + + + 0.3 0 0 + 0 0 -0.1 + + + + + + diff --git a/test/worlds/odometry_noise.sdf b/test/worlds/odometry_noise.sdf new file mode 100644 index 0000000000..e5d158f5a5 --- /dev/null +++ b/test/worlds/odometry_noise.sdf @@ -0,0 +1,231 @@ + + + + + + 0.001 + 0 + + + + + + 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 + + + + + + + 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 + + + + + + + 10 -10 0 0 0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 1 + + + + + + diff --git a/test/worlds/odometry_offset.sdf b/test/worlds/odometry_offset.sdf new file mode 100644 index 0000000000..ec5468ff63 --- /dev/null +++ b/test/worlds/odometry_offset.sdf @@ -0,0 +1,232 @@ + + + + + + 0.001 + 0 + + + + + + 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 + + + + + + + 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 + + + + + + + 10 -10 0 0 0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 1 -1 0 + 1.5708 0 0 + + + + + + diff --git a/test/worlds/sensors_system_battery.sdf b/test/worlds/sensors_system_battery.sdf new file mode 100644 index 0000000000..3c747c60ea --- /dev/null +++ b/test/worlds/sensors_system_battery.sdf @@ -0,0 +1,115 @@ + + + + + 0.001 + 0 + + + + + + + ogre2 + true + + + + 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 + + + + 0 0 0 0 0 0 + false + + 0 0 0.5 0 0 0 + + + + 1 1 1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + + 0 0 0.5 0 0 0 + + + + 1 1 1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + depth_camera + + + + + + linear_battery + 12.592 + 12.694 + -3.1424 + 0.01 + 0.01 + 0.061523 + 1.9499 + + 500 + /battery/discharge + + + + + diff --git a/test/worlds/shapes_scene_broadcaster_only.sdf b/test/worlds/shapes_scene_broadcaster_only.sdf new file mode 100644 index 0000000000..dff73115c5 --- /dev/null +++ b/test/worlds/shapes_scene_broadcaster_only.sdf @@ -0,0 +1,261 @@ + + + + + 0.001 + 0 + + + + + 1000 + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 2 3 0 0 1 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + 0.1 0.1 0.1 0 0 0 + + 0.11 0.11 0.11 0 0 0 + + + 3 4 5 + + + + + + 1150 + 0.12 0.12 0.12 0 0 0 + + + 1 2 3 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + -1 -2 -3 0 0 1 + + 0.2 0.2 0.2 0 0 0 + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + 0.21 0.21 0.21 0 0 0 + + + 0.2 + 0.1 + + + + + + 1654 + 0.22 0.22 0.22 0 0 0 + + + 2.1 + 10.2 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 0 0 0 0 1 + + 0.3 0.3 0.3 0 0 0 + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + 0.31 0.31 0.31 0 0 0 + + + 23.4 + + + + + + 50 + 0.32 0.32 0.32 0 0 0 + 0.5 + false + + + 1.2 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + -4 -5 -6 0 0 1 + + 0.5 0.5 0.5 0 0 0 + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + 0.51 0.51 0.51 0 0 0 + + + 0.23 + 0.14 + + + + + 6.54 + 0.52 0.52 0.52 0 0 0 + + + 2.12 + 1.23 + + + + 0 0 1 1 + 0 0 1 1 + 0 1 0 1 + + + + + + + 4 5 6 0 0 1 + + 0.8 0.8 0.8 0 0 0 + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + 0.81 0.81 0.81 0 0 0 + + + 0.4 0.6 1.6 + + + + + 3.21 + 0.82 0.82 0.82 0 0 0 + 0.5 + false + + + 0.4 0.6 1.6 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index a41255775a..74988a1c5c 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -36,8 +36,10 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests +* \subpage model_photo_shoot "Model Photo Shoot" Taking perspective, top, front, and side pictures of a model. * \subpage spherical_coordinates "Spherical coordinates": Working with latitude and longitude * \subpage python_interfaces Python interfaces +* \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. **Migration from Gazebo classic** diff --git a/tutorials/headless_rendering.md b/tutorials/headless_rendering.md new file mode 100644 index 0000000000..124efa8df0 --- /dev/null +++ b/tutorials/headless_rendering.md @@ -0,0 +1,76 @@ +\page headless_rendering Headless Rendering + +It is often desirable to run simulation on a remote computer, such as +a computer managed by cloud provider, in order to paralellize work or access +specific compute resources. Simulated sensors that require GPU access have +historically been difficult to use on a remote computer due to OpenGL's +X server requirement on linux systems. This issue can be resolved through +installation and proper configuration of X, but the steps can be complex and +error prone. + +An easier solution is through the use of [EGL](https://www.khronos.org/egl), which allows for the the creation of rendering surfaces without an X server. Ignition Gazebo has added support for EGL via the `--headless-rendering` command line option. Use of EGL is only available with OGRE2. + +Example usage: + +``` +ign gazebo -v 4 -s --headless-rendering sensors_demo.sdf +``` + +If you are using Ignition Gazebo as a library, then you can configure the +server to use headless rendering through the +`ServerConfig::SetHeadlessRendering(bool)` function. Make sure your SDF +world uses OGRE2. + +## AWS Example + +This example will guide you through the process of launching and configuring +an AWS GPU instance with Gazebo running headless. A GPU instance is +recommended when sensors that require a render engine are used. It is +possible to use a machine without a GPU, in which case OGRE will revert to +software rendering. You can read more about [OGRE's EGL implementation +here](https://www.ogre3d.org/2021/02/06/ogre-2-2-5-cerberus-released-and-egl-headless-support). + +1. Go to the [AWS EC2 service](https://console.aws.amazon.com/ec2) +2. Click the `Launch Instance` button in the upper right. +3. Select `Ubuntu Server` version 20.04 or greater from the AMI list. +4. Choose a GPU enabled instance type, such as `g3.4xlarge`. +5. Enable `Auto-assign Public IP` on the `Configure Instance Details` step. + This is not the best practice, but it simplifies this tutorial. +6. Add around 200GB storage to your instance on the `Add Storage` step. +7. Enable ssh source `Anywhere` on the `Configure Security Group` step. +8. Review and launch your instance. Make sure to setup a key pair in the + popup that appears after clicking `Launch`. + 1. You can configure other options as needed. Review the [AWS + documentation](https://docs.aws.amazon.com/AWSEC2/latest/UserGuide/EC2_GetStarted.html) for additional help. +9. Select the newly launched instance on the EC2 dashboard, and take note of + the `Public IPv4 address`. +10. SSH into your new machine instance. + ``` + ssh -i SSH_PEM_FILE_USED_DURING_LAUNCH ubuntu@EC_INSTANCE_PUBLIC_IP + ``` +12. Install Ubuntu drivers, which will install nvidia drivers: + ``` + sudo apt-get update + sudo apt install ubuntu-drivers-common + sudo ubuntu-drivers install + ``` +13. Add the `ubuntu` user to the `render` group, which is required to access + to the dri interfaces. + ``` + sudo usermod -a -G render ubuntu + ``` +14. Reboot the machine and log back in. + ``` + sudo reboot + ``` +11. [Install Gazebo](https://ignitionrobotics.org/docs/latest/install). +12. Run a Gazebo world that uses OGRE2 with camera sensors using headless rendering. This will enable EGL. + ``` + ign gazebo -v 4 -s -r --headless-rendering sensors_demo.sdf + ``` +13. Check that simulation is producing sensor data by ssh'ing into the EC2 + instance from a new terminal and echoing a sensor topic. + ``` + ssh -i SSH_PEM_FILE_USED_DURING_LAUNCH ubuntu@EC_INSTANCE_PUBLIC_IP + ign topic -et /thermal_camera + ``` diff --git a/tutorials/model_photo_shoot.md b/tutorials/model_photo_shoot.md new file mode 100644 index 0000000000..7a1874e9a5 --- /dev/null +++ b/tutorials/model_photo_shoot.md @@ -0,0 +1,106 @@ +\page model_photo_shoot Model Photo Shoot + +## Using the model photo shot plugin + +Ignition Gazebo offers a model photo taking tool that will take perspective, +top, front, and both sides pictures of a model. You can test the demo world +in Ignition Gazebo, located at `examples/worlds/model_photo_shoot.sdf`, by +running the following command: + +``` +ign gazebo -s -r -v 4 --iterations 50 model_photo_shoot.sdf +``` + +This will start Ignition Gazebo server, load the model and the plugin, take the +pictures and shutdown after 50 iterations. The pictures can be found at the +same location where the command was issued. + +## Model Photo Shoot configurations + +SDF is used to load and configure the `Model Photo Shoot` plugin. The demo SDF +contains a good example of the different options and other related plugins: + +1. The physics plugin: + +``` + + +``` + +A physics plugin is needed only if the `` option is to +be used. This will allow the `Model Photo Shoot` plugin to set the joints +to random positions. + +2. The render engine plugin: + +``` + + ogre2 + 1, 1, 1 + +``` + +A render plugin is needed to render the image. If `ogre2` is used, as shown in +the example, the `` tag can be used to set the background +of the pictures taken by the plugin. Please note that lights added by the +plugin will also affect the final resulting background color on the images. + +3. The model and the photo shoot plugin: + +``` + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + + poses.txt + true + + +``` + +The model is loaded through the `` tag. Then the `model photo shoot` +plugin and its options are specified: + +* ``: (optional) Location to store the camera +translation, scaling data and joints position (if using the +`` option) that can be used to replicate the +pictures using other systems. +* ``: (optional) When set to `true` the joints in the model +will be set to random positions prior to taking the pictures. + +4. Camera sensor: + +``` + + + 0 0 0 0 0 0 + + + 1.047 + + 960 + 540 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + true + +``` + +A `camera sensor` must be added as it will be used by the plugin to take the +pictures. This allows plugin users to set the different parameters of the +camera to their desired values. diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 0055886547..477a95dc29 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -9,35 +9,38 @@ step simulation and check entities and components. - **Step 1**: Load a world with a fixture - ```python - file_path = os.path.dirname(os.path.realpath(__file__)) - helper = HelperFixture(os.path.join(file_path, 'gravity.sdf')) - ``` +```{.python} +file_path = os.path.dirname(os.path.realpath(__file__)) +helper = HelperFixture(os.path.join(file_path, 'gravity.sdf')) +``` - **Step 2**: Write your `preupdate`, `update` or `postupdate` code: - ```python - def on_post_udpate_cb(_info, _ecm): - # - ... - ``` + +```python +def on_post_udpate_cb(_info, _ecm): + # + ... +``` - **Step 3**: Register the function. - ```python - helper.on_post_update(on_post_udpate_cb) - ``` +```python +helper.on_post_update(on_post_udpate_cb) +``` - **Step 4**: Be sure to call finalize before running the server. - ```python - helper.finalize() - ``` + +```python +helper.finalize() +``` - **Step 5**: Run the server - ```python - server.run(False, 1000, False) - while(server.is_running()): - time.sleep(0.1) - ``` + +```python +server.run(False, 1000, False) +while(server.is_running()): + time.sleep(0.1) +``` # Run the example @@ -45,11 +48,11 @@ In the [examples/scripts/python_api](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo6/examples/scripts/python_api) folder there is a Python script that shows how to make use of this API. -> If you compiled Ignition Gazebo from source you should modify your `PYTHONPATH`: -> -> ```bash -> export PYTHONPATH=$PYTHONPATH:/install/lib/python -> ``` +If you compiled Ignition Gazebo from source you should modify your `PYTHONPATH`: + +```bash +export PYTHONPATH=$PYTHONPATH:/install/lib/python +``` Now you can run the example: