diff --git a/CMakeLists.txt b/CMakeLists.txt index 3caa9b7dcc..ce24763e77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-gui -ign_find_package(ignition-gui4 REQUIRED) +ign_find_package(ignition-gui4 REQUIRED VERSION 4.1) set(IGN_GUI_VER ${ignition-gui4_VERSION_MAJOR}) ign_find_package (Qt5 COMPONENTS diff --git a/examples/worlds/logical_audio_sensor_plugin.sdf b/examples/worlds/logical_audio_sensor_plugin.sdf new file mode 100644 index 0000000000..9b93d11f99 --- /dev/null +++ b/examples/worlds/logical_audio_sensor_plugin.sdf @@ -0,0 +1,277 @@ + + + + + + 0.001 + 1.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 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 1 + .5 0 0 0 0 0 + linear + sphere + 1.0 + 5.0 + .8 + true + 10 + + + + + + 3 3 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + 1 + 0 0 0 0 0 0 + linear + sphere + 5.0 + 100.0 + .6 + false + 0 + + + + + + 0 2 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + 1 + 0 .5 0 0 0 0 + .4 + + + + + + -5 4 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + 1 + 0.5 0.5 0.5 0 0 0 + .3 + + + + + diff --git a/include/ignition/gazebo/components/LogicalAudio.hh b/include/ignition/gazebo/components/LogicalAudio.hh new file mode 100644 index 0000000000..7b465e6178 --- /dev/null +++ b/include/ignition/gazebo/components/LogicalAudio.hh @@ -0,0 +1,325 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_LOGICALAUDIO_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LOGICALAUDIO_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace logical_audio +{ + /// \brief Audio source attenuation functions. + /// AttenuationFunction::Undefined is used to indicate that an + /// attenuation function has not been defined yet. + enum class AttenuationFunction { LINEAR, UNDEFINED }; + + /// \brief Audio source attenuation shapes. + /// AttenuationShape::Undefined is used to indicate that an + /// attenuation shape has not been defined yet. + enum class AttenuationShape { SPHERE, UNDEFINED }; + + /// \brief Properties of a logical audio source. + /// A source also has a pose, which can be stored as a component of a + /// source entity via ignition::gazebo::components::Pose + struct Source + { + /// \brief The source's id + unsigned int id; + + /// \brief The source's attenuation function + logical_audio::AttenuationFunction attFunc; + + /// \brief The source's attenuation shape + logical_audio::AttenuationShape attShape; + + /// \brief The source's inner radius, which should be >= 0 + double innerRadius; + + /// \brief The source's falloff distance, which should be + /// greater than Source::innerRadius + double falloffDistance; + + /// \brief The source's emission volume, which should be + /// between 0.0 (0% volume) and 1.0 (100% volume) + double emissionVolume; + + public: bool operator==(const Source &_source) const + { + return this->id == _source.id; + } + + public: bool operator!=(const Source &_source) const + { + return !(*this == _source); + } + }; + + /// \brief A source's playing information. + /// Useful for keeping track of when to start/stop playing a source. + struct SourcePlayInfo + { + /// \brief Constructor + SourcePlayInfo() : startTime() + { + } + + /// \brief Whether the source is currently playing or not + bool playing{false}; + + /// \brief How long the source should play for, in seconds. + /// Setting this to 0 means the source has a play duration of infinity + std::chrono::seconds playDuration; + + /// \brief The time at which the source most recently started playing + std::chrono::steady_clock::duration startTime; + + public: bool operator==(const SourcePlayInfo &_sourcePlayInfo) const + { + return (this->playing == _sourcePlayInfo.playing) && + (this->playDuration == _sourcePlayInfo.playDuration) && + (this->startTime == _sourcePlayInfo.startTime); + } + + public: bool operator!=(const SourcePlayInfo &_sourcePlayInfo) const + { + return !(*this == _sourcePlayInfo); + } + }; + + /// \brief Properties of a logical audio microphone. + /// A microphone also has a pose, which can be stored as a component of a + /// microphone entity via ignition::gazebo::components::Pose + struct Microphone + { + /// \brief The microphone's id + unsigned int id; + + /// \brief The minimum volume this microphone can detect. + /// This should be a value between 0.0 (0% volume) and 1.0 (100% volume) + double volumeDetectionThreshold; + + public: bool operator==(const Microphone &_microphone) const + { + return this->id == _microphone.id; + } + + public: bool operator!=(const Microphone &_microphone) const + { + return !(*this == _microphone); + } + }; +} + +namespace serializers +{ + /// \brief Output stream overload for logical_audio::AttenuationFunction + inline std::ostream &operator<<(std::ostream &_out, + const logical_audio::AttenuationFunction &_func) + { + auto temp = static_cast(_func); + _out << temp; + return _out; + } + + /// \brief Input stream overload for logical_audio::AttenuationFunction + inline std::istream &operator>>(std::istream &_in, + logical_audio::AttenuationFunction &_func) + { + unsigned int temp = 0u; + if (_in >> temp) + _func = static_cast(temp); + return _in; + } + + /// \brief Output stream overload for logical_audio::AttenuationShape + inline std::ostream &operator<<(std::ostream &_out, + const logical_audio::AttenuationShape &_shape) + { + auto temp = static_cast(_shape); + _out << temp; + return _out; + } + + /// \brief Input stream overload for logical_audio::AttenuationShape + inline std::istream &operator>>(std::istream &_in, + logical_audio::AttenuationShape &_shape) + { + unsigned int temp = 0u; + if (_in >> temp) + _shape = static_cast(temp); + return _in; + } + + /// \brief Output stream overload for std::chrono::steady_clock::duration + inline std::ostream &operator<<(std::ostream &_out, + const std::chrono::steady_clock::duration &_dur) + { + _out << std::chrono::duration_cast( + _dur).count(); + return _out; + } + + /// \brief Input stream overload for std::chrono::steady_clock::duration + inline std::istream &operator>>(std::istream &_in, + std::chrono::steady_clock::duration &_dur) + { + int64_t time; + _in >> time; + _dur = std::chrono::duration(time); + return _in; + } + + /// \brief Serializer for components::LogicalAudioSource object + class LogicalAudioSourceSerializer + { + /// \brief Serialization for logical_audio::Source + /// \param[out] _out Output stream + /// \param[in] _source Object for the stream + /// \return The stream + public: static std::ostream &Serialize(std::ostream &_out, + const logical_audio::Source &_source) + { + _out << _source.id << " " << _source.attFunc << " " << _source.attShape + << " " << _source.innerRadius << " " << _source.falloffDistance + << " " << _source.emissionVolume; + return _out; + } + + /// \brief Deserialization for logical_audio::Source + /// \param[in] _in Input stream + /// \param[out] _source The object to populate + /// \return The stream + public: static std::istream &Deserialize(std::istream &_in, + logical_audio::Source &_source) + { + _in >> _source.id >> _source.attFunc >> _source.attShape + >> _source.innerRadius >> _source.falloffDistance + >> _source.emissionVolume; + return _in; + } + }; + + /// \brief Serializer for components::LogicalAudioSourcePlayInfo object + class LogicalAudioSourcePlayInfoSerializer + { + /// \brief Serialization for logical_audio::SourcePlayInfo + /// \param[out] _out Output stream + /// \param[in] _playInfo Object for the stream + /// \return The stream + public: static std::ostream &Serialize(std::ostream &_out, + const logical_audio::SourcePlayInfo &_playInfo) + { + _out << _playInfo.playing << " " << _playInfo.playDuration.count() << " " + << _playInfo.startTime; + return _out; + } + + /// \brief Deserialization for logical_audio::SourcePlayInfo + /// \param[in] _in Input stream + /// \param[out] _playInfo The object to populate + /// \return The stream + public: static std::istream &Deserialize(std::istream &_in, + logical_audio::SourcePlayInfo &_playInfo) + { + uint64_t count; + _in >> _playInfo.playing >> count >> _playInfo.startTime; + _playInfo.playDuration = std::chrono::seconds(count); + return _in; + } + }; + + /// \brief Serializer for components::LogicalMicrophone object + class LogicalMicrophoneSerializer + { + /// \brief Serialization for logical_audio::Microphone + /// \param[out] _out Output stream + /// \param[in] _mic Object for the stream + /// \return The stream + public: static std::ostream &Serialize(std::ostream &_out, + const logical_audio::Microphone &_mic) + { + _out << _mic.id << " " << _mic.volumeDetectionThreshold; + return _out; + } + + /// \brief Deserialization for logical_audio::Microphone + /// \param[in] _in Inout stream + /// \param[out] _mic The object to populate + public: static std::istream &Deserialize(std::istream &_in, + logical_audio::Microphone &_mic) + { + _in >> _mic.id >> _mic.volumeDetectionThreshold; + return _in; + } + }; +} + +// using separate namespace blocks so all components appear in Doxygen +// (appears as if Doxygen can't parse multiple components in a single +// namespace block since IGN_GAZEBO_REGISTER_COMPONENT doesn't have a +// trailing semicolon) +namespace components +{ + /// \brief A component that contains a logical audio source, which is + /// represented by logical_audio::Source + using LogicalAudioSource = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LogicalAudioSource", + LogicalAudioSource) +} + +namespace components +{ + /// \brief A component that contains a logical audio source's playing + /// information, which is represented by logical_audio::SourcePlayInfo + using LogicalAudioSourcePlayInfo = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.LogicalAudioSourcePlayInfo", + LogicalAudioSourcePlayInfo) +} + +namespace components +{ + /// \brief A component that contains a logical microphone, which is + /// represented by logical_audio::Microphone + using LogicalMicrophone = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LogicalMicrophone", + LogicalMicrophone) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index ee87528162..027baf355c 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -63,6 +63,7 @@ namespace traits template auto CompareData = [](const DataType &_a, const DataType &_b) -> bool { + // cppcheck-suppress syntaxError if constexpr (std::is_same::value) { return math::equal(_a, _b); diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/include/ignition/gazebo/gui/GuiRunner.hh index 564ac183f0..a60f77274c 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/include/ignition/gazebo/gui/GuiRunner.hh @@ -53,11 +53,9 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject /// \brief Make a new state request to the server. public slots: void RequestState(); - /// \brief Callback for the state service. + /// \brief Callback for the async state service. /// \param[in] _res Response containing new state. - /// \param[in] _result True if successful. - private: void OnStateService(const msgs::SerializedStepMap &_res, - const bool _result); + private: void OnStateAsyncService(const msgs::SerializedStepMap &_res); /// \brief Callback when a new state is received from the server. /// \param[in] _msg New state message. diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index e7f4c0b15f..775487f4fa 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -70,16 +70,16 @@ namespace ignition struct WorldControl { /// \brief True to pause simulation. - /// cppcheck-suppress unusedStructMember + // cppcheck-suppress unusedStructMember bool pause{false}; // NOLINT /// \biref Run a given number of simulation iterations. - /// cppcheck-suppress unusedStructMember + // cppcheck-suppress unusedStructMember uint64_t multiStep{0u}; // NOLINT /// \brief Reset simulation back to time zero. Rewinding resets sim time, /// real time and iterations. - /// cppcheck-suppress unusedStructMember + // cppcheck-suppress unusedStructMember bool rewind{false}; // NOLINT /// \brief Sim time to jump to. A negative value means don't seek. diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 4456f3c39e..565838a06c 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -59,7 +59,16 @@ GuiRunner::~GuiRunner() = default; ///////////////////////////////////////////////// void GuiRunner::RequestState() { - this->node.Request(this->stateTopic, &GuiRunner::OnStateService, this); + // set up service for async state response callback + std::string id = std::to_string(gui::App()->applicationPid()); + std::string reqSrv = + this->node.Options().NameSpace() + "/" + id + "/state_async"; + this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); + ignition::msgs::StringMsg req; + req.set_data(reqSrv); + + // send async state request + this->node.Request(this->stateTopic + "_async", req); } ///////////////////////////////////////////////// @@ -77,17 +86,17 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) } ///////////////////////////////////////////////// -void GuiRunner::OnStateService(const msgs::SerializedStepMap &_res, - const bool _result) +void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) { - if (!_result) - { - ignerr << "Service call failed for [" << this->stateTopic << "]" - << std::endl; - return; - } this->OnState(_res); + // todo(anyone) store reqSrv string in a member variable and use it here + // and in RequestState() + std::string id = std::to_string(gui::App()->applicationPid()); + std::string reqSrv = + this->node.Options().NameSpace() + "/" + id + "/state_async"; + this->node.UnadvertiseSrv(reqSrv); + // Only subscribe to periodic updates after receiving initial state if (this->node.SubscribedTopics().empty()) this->node.Subscribe(this->stateTopic, &GuiRunner::OnState, this); diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 2da32427c6..56bb1b13f3 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -90,6 +90,7 @@ add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) add_subdirectory(shapes) +add_subdirectory(tape_measure) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index 20c56f0cb5..e2b7084e8a 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -18,7 +18,12 @@ #include #include +#include #include +#include +#include +#include +#include #include #include #include diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 228a140a57..4f882de9f7 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -81,12 +81,14 @@ namespace gazebo struct EntityInfo { /// \brief Entity ID + // cppcheck-suppress unusedStructMember unsigned int entity; /// \brief Entity name QString name; /// \brief Parent ID + // cppcheck-suppress unusedStructMember unsigned int parentEntity; /// \brief Entity type diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 8d47713910..a567b3cb8f 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.hh b/src/gui/plugins/resource_spawner/ResourceSpawner.hh index e2fa9478c8..d95e5727cb 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.hh +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.hh @@ -76,6 +76,7 @@ namespace gazebo /// \brief True if the user is currently observing fuel resources and false /// if the user is currently observing local resources. + // cppcheck-suppress unusedStructMember bool isFuel = false; }; diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 8522b91259..8854e38de6 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -55,6 +55,7 @@ #include #include +#include #include #include @@ -314,6 +315,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Flag to indicate whether the z key is currently being pressed public: bool zPressed = false; + /// \brief Flag to indicate whether the escape key has been released. + public: bool escapeReleased = false; + /// \brief ID of thread where render calls can be made. public: std::thread::id renderThreadId; @@ -672,6 +676,17 @@ void IgnRenderer::Render() } } + // Escape action, clear all selections and terminate any + // spawned previews if escape button is released + { + if (this->dataPtr->escapeReleased) + { + this->DeselectAllEntities(true); + this->TerminateSpawnPreview(); + this->dataPtr->escapeReleased = false; + } + } + if (ignition::gui::App()) { gui::events::Render event; @@ -767,12 +782,44 @@ Entity IgnRenderer::UniqueId() void IgnRenderer::HandleMouseEvent() { std::lock_guard lock(this->dataPtr->mutex); + this->BroadcastHoverPos(); + this->BroadcastLeftClick(); this->HandleMouseContextMenu(); this->HandleModelPlacement(); this->HandleMouseTransformControl(); this->HandleMouseViewControl(); } +///////////////////////////////////////////////// +void IgnRenderer::BroadcastHoverPos() +{ + if (this->dataPtr->hoverDirty) + { + math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseHoverPos); + + ignition::gui::events::HoverToScene hoverToSceneEvent(pos); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &hoverToSceneEvent); + } +} + +///////////////////////////////////////////////// +void IgnRenderer::BroadcastLeftClick() +{ + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && + !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) + { + math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); + + ignition::gui::events::LeftClickToScene leftClickToSceneEvent(pos); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &leftClickToSceneEvent); + } +} + ///////////////////////////////////////////////// void IgnRenderer::HandleMouseContextMenu() { @@ -924,6 +971,9 @@ void IgnRenderer::HandleKeyRelease(QKeyEvent *_e) case Qt::Key_Z: this->dataPtr->zPressed = false; break; + case Qt::Key_Escape: + this->dataPtr->escapeReleased = true; + break; default: break; } @@ -2361,6 +2411,8 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Camera pose topic advertised on [" << this->dataPtr->cameraPoseTopic << "]" << std::endl; + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->installEventFilter(this); } @@ -2562,7 +2614,26 @@ void RenderWindowItem::SetScaleSnap(const math::Vector3d &_scale) ///////////////////////////////////////////////// bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->HandleKeyPress(keyEvent); + } + } + else if (_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->HandleKeyRelease(keyEvent); + } + } + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) { auto selectedEvent = reinterpret_cast(_event); @@ -2838,13 +2909,13 @@ void RenderWindowItem::wheelEvent(QWheelEvent *_e) } //////////////////////////////////////////////// -void RenderWindowItem::keyPressEvent(QKeyEvent *_e) +void RenderWindowItem::HandleKeyPress(QKeyEvent *_e) { this->dataPtr->renderThread->ignRenderer.HandleKeyPress(_e); } //////////////////////////////////////////////// -void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) +void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e) { this->dataPtr->renderThread->ignRenderer.HandleKeyRelease(_e); @@ -2857,8 +2928,6 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } - this->DeselectAllEntities(true); - this->dataPtr->renderThread->ignRenderer.TerminateSpawnPreview(); } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 0ecd0c69ec..0947cffbc9 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -351,6 +351,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Handle model placement requests private: void HandleModelPlacement(); + /// \brief Broadcasts the currently hovered 3d scene location. + private: void BroadcastHoverPos(); + + /// \brief Broadcasts a left click within the scene + private: void BroadcastLeftClick(); + /// \brief Generate a unique entity id. /// \return The unique entity id private: Entity UniqueId(); @@ -606,6 +612,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); + /// \brief Handle key press event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyPress(QKeyEvent *_e); + + /// \brief Handle key release event for snapping + /// \param[in] _e The key event to process. + public: void HandleKeyRelease(QKeyEvent *_e); + // Documentation inherited protected: void mousePressEvent(QMouseEvent *_e) override; @@ -618,12 +632,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { // Documentation inherited protected: void wheelEvent(QWheelEvent *_e) override; - // Documentation inherited - protected: void keyPressEvent(QKeyEvent *_e) override; - - // Documentation inherited - protected: void keyReleaseEvent(QKeyEvent *_e) override; - /// \brief Overrides the paint event to render the render engine /// camera view /// \param[in] _oldNode The node passed in previous updatePaintNode diff --git a/src/gui/plugins/tape_measure/CMakeLists.txt b/src/gui/plugins/tape_measure/CMakeLists.txt new file mode 100644 index 0000000000..4b5322fc2a --- /dev/null +++ b/src/gui/plugins/tape_measure/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(TapeMeasure + SOURCES TapeMeasure.cc + QT_HEADERS TapeMeasure.hh + PRIVATE_LINK_LIBS + ${IGNITION-RENDERING_LIBRARIES} +) diff --git a/src/gui/plugins/tape_measure/TapeMeasure.cc b/src/gui/plugins/tape_measure/TapeMeasure.cc new file mode 100644 index 0000000000..adbec33d47 --- /dev/null +++ b/src/gui/plugins/tape_measure/TapeMeasure.cc @@ -0,0 +1,301 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "TapeMeasure.hh" + +namespace ignition::gazebo +{ + class TapeMeasurePrivate + { + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief True if currently measuring, else false. + public: bool measure = false; + + /// \brief The id of the start point marker. + public: const int kStartPointId = 1; + + /// \brief The id of the end point marker. + public: const int kEndPointId = 2; + + /// \brief The id of the line marker. + public: const int kLineId = 3; + + /// \brief The id of the start or end point marker that is currently + /// being placed. This is primarily used to track the state machine of + /// the plugin. + public: int currentId = kStartPointId; + + /// \brief The location of the placed starting point of the tape measure + /// tool, only set when the user clicks to set the point. + public: ignition::math::Vector3d startPoint = + ignition::math::Vector3d::Zero; + + /// \brief The location of the placed ending point of the tape measure + /// tool, only set when the user clicks to set the point. + public: ignition::math::Vector3d endPoint = ignition::math::Vector3d::Zero; + + /// \brief The color to set the marker when hovering the mouse over the + /// scene. + public: ignition::math::Color + hoverColor{ignition::math::Color(0.2, 0.2, 0.2, 0.5)}; + + /// \brief The color to draw the marker when the user clicks to confirm + /// its location. + public: ignition::math::Color + drawColor{ignition::math::Color(0.2, 0.2, 0.2, 1.0)}; + + /// \brief A set of the currently placed markers. Used to make sure a + /// non-existent marker is not deleted. + public: std::unordered_set placedMarkers; + + /// \brief The current distance between the two points. This distance + /// is updated as the user hovers the end point as well. + public: double distance = 0.0; + + /// \brief The namespace that the markers for this plugin are placed in. + public: std::string ns = "tape_measure"; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +TapeMeasure::TapeMeasure() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +TapeMeasure::~TapeMeasure() = default; + +///////////////////////////////////////////////// +void TapeMeasure::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Tape measure"; + + ignition::gui::App()->findChild + ()->installEventFilter(this); + ignition::gui::App()->findChild + ()->QuickWindow()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void TapeMeasure::OnMeasure() +{ + this->Measure(); +} + +///////////////////////////////////////////////// +void TapeMeasure::Measure() +{ + this->Reset(); + this->dataPtr->measure = true; + QGuiApplication::setOverrideCursor(Qt::CrossCursor); +} + +///////////////////////////////////////////////// +void TapeMeasure::OnReset() +{ + this->Reset(); +} + +///////////////////////////////////////////////// +void TapeMeasure::Reset() +{ + this->DeleteMarker(this->dataPtr->kStartPointId); + this->DeleteMarker(this->dataPtr->kEndPointId); + this->DeleteMarker(this->dataPtr->kLineId); + + this->dataPtr->currentId = this->dataPtr->kStartPointId; + this->dataPtr->startPoint = ignition::math::Vector3d::Zero; + this->dataPtr->endPoint = ignition::math::Vector3d::Zero; + this->dataPtr->distance = 0.0; + this->dataPtr->measure = false; + this->newDistance(); + QGuiApplication::restoreOverrideCursor(); +} + +///////////////////////////////////////////////// +double TapeMeasure::Distance() +{ + return this->dataPtr->distance; +} + +///////////////////////////////////////////////// +void TapeMeasure::DeleteMarker(int _id) +{ + if (this->dataPtr->placedMarkers.find(_id) == + this->dataPtr->placedMarkers.end()) + return; + + // Delete the previously created marker + ignition::msgs::Marker markerMsg; + markerMsg.set_ns(this->dataPtr->ns); + markerMsg.set_id(_id); + markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); + this->dataPtr->node.Request("/marker", markerMsg); + this->dataPtr->placedMarkers.erase(_id); +} + +///////////////////////////////////////////////// +void TapeMeasure::DrawPoint(int _id, + ignition::math::Vector3d &_point, ignition::math::Color &_color) +{ + this->DeleteMarker(_id); + + ignition::msgs::Marker markerMsg; + markerMsg.set_ns(this->dataPtr->ns); + markerMsg.set_id(_id); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::SPHERE); + ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); + ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(0.1, 0.1, 0.1)); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0)); + + this->dataPtr->node.Request("/marker", markerMsg); + this->dataPtr->placedMarkers.insert(_id); +} + +///////////////////////////////////////////////// +void TapeMeasure::DrawLine(int _id, ignition::math::Vector3d &_startPoint, + ignition::math::Vector3d &_endPoint, ignition::math::Color &_color) +{ + this->DeleteMarker(_id); + + ignition::msgs::Marker markerMsg; + markerMsg.set_ns(this->dataPtr->ns); + markerMsg.set_id(_id); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); + ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); + ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); + ignition::msgs::Set(markerMsg.add_point(), _startPoint); + ignition::msgs::Set(markerMsg.add_point(), _endPoint); + + this->dataPtr->node.Request("/marker", markerMsg); + this->dataPtr->placedMarkers.insert(_id); +} + +///////////////////////////////////////////////// +bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::HoverToScene::kType) + { + auto hoverToSceneEvent = + reinterpret_cast(_event); + + // This event is called in Scene3d's RenderThread, so it's safe to make + // rendering calls here + if (this->dataPtr->measure && hoverToSceneEvent) + { + ignition::math::Vector3d point = hoverToSceneEvent->Point(); + this->DrawPoint(this->dataPtr->currentId, point, + this->dataPtr->hoverColor); + + // If the user is currently choosing the end point, draw the connecting + // line and update the new distance. + if (this->dataPtr->currentId == this->dataPtr->kEndPointId) + { + this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint, + point, this->dataPtr->hoverColor); + this->dataPtr->distance = this->dataPtr->startPoint.Distance(point); + this->newDistance(); + } + } + } + else if (_event->type() == ignition::gui::events::LeftClickToScene::kType) + { + auto leftClickToSceneEvent = + reinterpret_cast(_event); + + // This event is called in Scene3d's RenderThread, so it's safe to make + // rendering calls here + if (this->dataPtr->measure && leftClickToSceneEvent) + { + ignition::math::Vector3d point = leftClickToSceneEvent->Point(); + this->DrawPoint(this->dataPtr->currentId, point, + this->dataPtr->drawColor); + // If the user is placing the start point, update its position + if (this->dataPtr->currentId == this->dataPtr->kStartPointId) + { + this->dataPtr->startPoint = point; + } + // If the user is placing the end point, update the end position, + // end the measurement state, and update the draw line and distance + else + { + this->dataPtr->endPoint = point; + this->dataPtr->measure = false; + this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint, + this->dataPtr->endPoint, this->dataPtr->drawColor); + this->dataPtr->distance = + this->dataPtr->startPoint.Distance(this->dataPtr->endPoint); + this->newDistance(); + QGuiApplication::restoreOverrideCursor(); + } + this->dataPtr->currentId = this->dataPtr->kEndPointId; + } + } + else if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent && keyEvent->key() == Qt::Key_M) + { + this->Reset(); + this->Measure(); + } + } + else if (_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent && keyEvent->key() == Qt::Key_Escape && + this->dataPtr->measure) + { + this->Reset(); + } + } + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::TapeMeasure, + ignition::gui::Plugin) diff --git a/src/gui/plugins/tape_measure/TapeMeasure.hh b/src/gui/plugins/tape_measure/TapeMeasure.hh new file mode 100644 index 0000000000..0da8719f2b --- /dev/null +++ b/src/gui/plugins/tape_measure/TapeMeasure.hh @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_GUI_TAPEMEASURE_HH_ +#define IGNITION_GAZEBO_GUI_TAPEMEASURE_HH_ + +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ + class TapeMeasurePrivate; + + /// \brief Provides buttons for the tape measure tool. + class TapeMeasure : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: TapeMeasure(); + + /// \brief Destructor + public: ~TapeMeasure() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Deletes the marker with the provided id within the + /// "tape_measure" namespace. + /// \param[in] _id The id of the marker + public: void DeleteMarker(int _id); + + /// \brief Resets all of the relevant data for this plugin. Called when + /// the user clicks the reset button and when the user starts a new + /// measurement. + public: void Reset(); + + /// \brief Starts a new measurement. Erases any previous measurement in + /// progress or already made. + public: void Measure(); + + /// \brief Draws a point marker. Called to display the start and end + /// point of the tape measure. + /// \param[in] _id The id of the marker + /// \param[in] _point The x, y, z coordinates of where to place the marker + /// \param[in] _color The rgba color to set the marker + public: void DrawPoint(int _id, + ignition::math::Vector3d &_point, + ignition::math::Color &_color); + + /// \brief Draws a line marker. Called to display the line between the + /// start and end point of the tape measure. + /// \param[in] _id The id of the marker + /// \param[in] _startPoint The x, y, z coordinates of the line start point + /// \param[in] _endPoint The x, y, z coordinates of the line end point + /// \param[in] _color The rgba color to set the marker + public: void DrawLine(int _id, + ignition::math::Vector3d &_startPoint, + ignition::math::Vector3d &_endPoint, + ignition::math::Color &_color); + + /// \brief Callback in Qt thread when the new measurement button is + /// clicked. + public slots: void OnMeasure(); + + /// \brief Callback in Qt thread when the reset button is clicked. + public slots: void OnReset(); + + /// \brief Callback in Qt thread to get the distance to display in the + /// gui window. + /// \return The distance between the start and end point of the measurement + public slots: double Distance(); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Signal fired when a new tape measure distance is set. + signals: void newDistance(); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/tape_measure/TapeMeasure.qml b/src/gui/plugins/tape_measure/TapeMeasure.qml new file mode 100644 index 0000000000..29362bcf09 --- /dev/null +++ b/src/gui/plugins/tape_measure/TapeMeasure.qml @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Window 2.2 +import QtQuick.Controls 2.1 +import QtQuick.Controls.Material 2.2 +import QtQuick.Controls.Material.impl 2.2 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/qml" + +ToolBar { + id: tapeMeasure + Layout.minimumWidth: 250 + Layout.minimumHeight: 100 + + property var distance: 0.0 + + function updateDistance() { + distance = TapeMeasure.Distance(); + } + + Connections { + target: TapeMeasure + onNewDistance: { + updateDistance(); + } + } + + background: Rectangle { + color: "transparent" + } + + ButtonGroup { + id: group + } + + RowLayout { + spacing: 1 + ToolButton { + id: select + checkable: true + checked: true + ButtonGroup.group: group + ToolTip.text: "Tape Measure Tool" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "tape_measure.png" + sourceSize.width: 36; + sourceSize.height: 36; + } + onClicked: { + TapeMeasure.OnMeasure(); + } + } + ToolButton { + id: reset + checkable: true + checked: true + ButtonGroup.group: group + ToolTip.text: "Reset measurement" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "trashcan.png" + sourceSize.width: 36; + sourceSize.height: 36; + } + onClicked: { + TapeMeasure.OnReset(); + } + } + Text { + text: qsTr(" Distance (m): " + distance.toFixed(3)) + font.pointSize: 14 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + } + } +} diff --git a/src/gui/plugins/tape_measure/TapeMeasure.qrc b/src/gui/plugins/tape_measure/TapeMeasure.qrc new file mode 100644 index 0000000000..e87212d276 --- /dev/null +++ b/src/gui/plugins/tape_measure/TapeMeasure.qrc @@ -0,0 +1,7 @@ + + + TapeMeasure.qml + tape_measure.png + trashcan.png + + diff --git a/src/gui/plugins/tape_measure/tape_measure.png b/src/gui/plugins/tape_measure/tape_measure.png new file mode 100644 index 0000000000..228e583442 Binary files /dev/null and b/src/gui/plugins/tape_measure/tape_measure.png differ diff --git a/src/gui/plugins/tape_measure/trashcan.png b/src/gui/plugins/tape_measure/trashcan.png new file mode 100644 index 0000000000..62edd38a51 Binary files /dev/null and b/src/gui/plugins/tape_measure/trashcan.png differ diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 461f2e7e1b..d4b1d8c94e 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -96,6 +96,8 @@ void TransformControl::LoadConfig(const tinyxml2::XMLElement *) ignition::gui::App()->findChild ()->installEventFilter(this); + ignition::gui::App()->findChild + ()->QuickWindow()->installEventFilter(this); } ///////////////////////////////////////////////// @@ -232,6 +234,27 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->snapToGrid = false; } } + else if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent->key() == Qt::Key_T) + { + this->activateTranslate(); + } + else if (keyEvent->key() == Qt::Key_R) + { + this->activateRotate(); + } + } + else if (_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent->key() == Qt::Key_Escape) + { + this->activateSelect(); + } + } + return QObject::eventFilter(_obj, _event); } diff --git a/src/gui/plugins/transform_control/TransformControl.hh b/src/gui/plugins/transform_control/TransformControl.hh index 64958e3927..3c8a4e5dd5 100644 --- a/src/gui/plugins/transform_control/TransformControl.hh +++ b/src/gui/plugins/transform_control/TransformControl.hh @@ -133,6 +133,15 @@ namespace gazebo /// \brief Notify that new snapping values have been set. signals: void newSnapValues(); + /// \brief Notify that selection has been activated + signals: void activateSelect(); + + /// \brief Notify that translation has been activated + signals: void activateTranslate(); + + /// \brief Notify that rotation has been activated + signals: void activateRotate(); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/transform_control/TransformControl.qml b/src/gui/plugins/transform_control/TransformControl.qml index 765506c45b..2563902daa 100644 --- a/src/gui/plugins/transform_control/TransformControl.qml +++ b/src/gui/plugins/transform_control/TransformControl.qml @@ -91,25 +91,19 @@ ToolBar { onNewSnapValues: updateSnapValues(); } - // TODO(anyone) enable scale button when support is added in ign-physics - // Shortcut { - // sequence: "S" - // onActivated: activateScale() - // } - - Shortcut { - sequence: "T" - onActivated: activateTranslate() + Connections { + target: TransformControl + onActivateSelect: activateSelect(); } - Shortcut { - sequence: "R" - onActivated: activateRotate() + Connections { + target: TransformControl + onActivateTranslate: activateTranslate(); } - Shortcut { - sequence: "Esc" - onActivated: activateSelect() + Connections { + target: TransformControl + onActivateRotate: activateRotate(); } RowLayout { diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 52668dc0f0..51cf04e10b 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -15,7 +15,10 @@ * */ +#include "VisualizeLidar.hh" + #include +#include #include #include @@ -56,7 +59,6 @@ #include "ignition/gazebo/Util.hh" #include "ignition/msgs/laserscan.pb.h" -#include "VisualizeLidar.hh" namespace ignition { diff --git a/src/ign.cc b/src/ign.cc index 77c1620481..09bb0dbf8a 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -73,6 +73,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( ignmsg << "Cached world found." << std::endl; worldPath = path; } + // cppcheck-suppress syntaxError else if (ignition::fuel_tools::Result result = fuelClient.DownloadWorld(ignition::common::URI(_pathToResource), path); result) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 3dd61d6e91..cf899f8476 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -88,6 +88,7 @@ add_subdirectory(joint_state_publisher) add_subdirectory(lift_drag) add_subdirectory(log) add_subdirectory(log_video_recorder) +add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) add_subdirectory(multicopter_motor_model) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index f3489f6f56..81a880a21a 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 70aed6c885..52c24ad0f9 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index c8101552f2..dfd78b7769 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include diff --git a/src/systems/follow_actor/FollowActor.cc b/src/systems/follow_actor/FollowActor.cc index 32d51ef60a..f55c65c8ce 100644 --- a/src/systems/follow_actor/FollowActor.cc +++ b/src/systems/follow_actor/FollowActor.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include diff --git a/src/systems/logical_audio_sensor_plugin/CMakeLists.txt b/src/systems/logical_audio_sensor_plugin/CMakeLists.txt new file mode 100644 index 0000000000..2579a278eb --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/CMakeLists.txt @@ -0,0 +1,21 @@ +gz_add_system(logicalaudiosensorplugin + SOURCES + LogicalAudioSensorPlugin.cc + LogicalAudio.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) + +set (gtest_sources + LogicalAudio_TEST.cc +) + +ign_build_tests(TYPE UNIT + SOURCES + ${gtest_sources} + LIB_DEPS + ${PROJECT_LIBRARY_TARGET_NAME}-logicalaudiosensorplugin-system +) diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudio.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudio.cc new file mode 100644 index 0000000000..3568bd3c4a --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudio.cc @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "LogicalAudio.hh" + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace logical_audio +{ + /// \brief A map to help convert user-input strings to the proper + /// attenuation function. + const std::unordered_map + kAttFuncMap {{"linear", AttenuationFunction::LINEAR}}; + + /// \brief A map to help convert user-input strings to the proper + /// attenuation shape. + const std::unordered_map + kAttShapeMap {{"sphere", AttenuationShape::SPHERE}}; + + ////////////////////////////////////////////////// + bool detect(double _volumeLevel, double _volumeDetectionThreshold) + { + // if the volume level is <= 0, this can't be detected + // (not even if _volumeDetectionThreshold is 0.0) + if (_volumeLevel < 0.00001) + return false; + + return _volumeLevel >= _volumeDetectionThreshold; + } + + ////////////////////////////////////////////////// + double computeVolume(bool _playing, + AttenuationFunction _attenuationFunc, + AttenuationShape _attenuationShape, + double _sourceEmissionVolume, + double _innerRadius, + double _falloffDistance, + const ignition::math::Pose3d &_sourcePose, + const ignition::math::Pose3d &_targetPose) + { + if (!_playing) + return 0.0; + + // make sure the source has a valid attenuation function and shape + if ((_attenuationFunc == AttenuationFunction::UNDEFINED) || + (_attenuationShape == AttenuationShape::UNDEFINED)) + return -1.0; + // make sure the audio source has a playing volume that's > 0 + else if (_sourceEmissionVolume < 0.00001) + return 0.0; + + auto dist = _sourcePose.Pos().Distance(_targetPose.Pos()); + + // Implementing AttenuationShape::SPHERE for now since that's + // the only attenuation shape that's available + if (dist <= _innerRadius) + return _sourceEmissionVolume; + else if (dist >= _falloffDistance) + return 0.0; + + // Implementing AttenuationFunction::LINEAR for now since that's + // the only attenuation function that's available. + // + // The equation below was calculated as follows: + // Point slope formula is y - y_1 = m * (x - x_1), rewritten as: + // y = (m * (x - x_1)) + y_1 + // The variables in the equation above are defined as: + // y = volume at _targetPose, m = slope, + // x = distance(_sourcePose, _targetPose), + // x_1 = _innerRadius, y_1 = _sourceEmissionVolume + // The slope (m) is defined as delta_y / delta_x, where y is volume and x + // is position. We have two (x,y) points: y is the source's output + // volume when x is _innerRadius, and y is 0 when x is _falloffDistance. + // So, we can calculate the slope using these two points. + double m = -_sourceEmissionVolume / (_falloffDistance - _innerRadius); + return (m * (dist - _innerRadius)) + _sourceEmissionVolume; + } + + ////////////////////////////////////////////////// + void setAttenuationFunction(AttenuationFunction &_attenuationFunc, + std::string _str) + { + std::transform(_str.begin(), _str.end(), + _str.begin(), ::tolower); + + auto iter = kAttFuncMap.find(_str); + if (iter != kAttFuncMap.end()) + _attenuationFunc = iter->second; + else + _attenuationFunc = AttenuationFunction::UNDEFINED; + } + + ////////////////////////////////////////////////// + void setAttenuationShape(AttenuationShape &_attenuationShape, + std::string _str) + { + std::transform(_str.begin(), _str.end(), + _str.begin(), ::tolower); + + auto iter = kAttShapeMap.find(_str); + if (iter != kAttShapeMap.end()) + _attenuationShape = iter->second; + else + _attenuationShape = AttenuationShape::UNDEFINED; + } + + ////////////////////////////////////////////////// + void validateInnerRadiusAndFalloffDistance(double &_innerRadius, + double &_falloffDistance) + { + if (_innerRadius < 0.0) + { + _innerRadius = 0.0; + } + + if (_falloffDistance <= _innerRadius) + { + _falloffDistance = _innerRadius + 1.0; + } + } + + ////////////////////////////////////////////////// + void validateVolumeLevel(double &_volumeLevel) + { + if (_volumeLevel < 0.0) + _volumeLevel = 0.0; + else if (_volumeLevel > 1.0) + _volumeLevel = 1.0; + } +} // namespace logical_audio +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudio.hh b/src/systems/logical_audio_sensor_plugin/LogicalAudio.hh new file mode 100644 index 0000000000..4835cd6b88 --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudio.hh @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_LOGICAL_AUDIO_SENSOR_PLUGIN_LOGICALAUDIO_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LOGICAL_AUDIO_SENSOR_PLUGIN_LOGICALAUDIO_HH_ + +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace logical_audio +{ + /// \brief Determines if an audio device can detect volume at a certain level. + /// \param[in] _volumeLevel The volume level that the microphone is + /// attempting to detect. This should be a value between 0.0 (no volume) and + /// 1.0 (maximum volume). + /// \param[in] _volumeDetectionThreshold The listening device's minimum audio + /// detection volume. This should be a value between 0.0 and 1.0. A device + /// with a low detection threshold can hear a wider range of audio than a + /// device with a higher detection threshold. + /// \return true if the listening device can detect volume at _volumeLevel, + /// false otherwise. + bool detect(double _volumeLevel, double _volumeDetectionThreshold); + + /// \brief Computes the volume level of an audio source at a certain location. + /// \note Users should call + /// logical_audio::ValidateInnerRadiusAndFalloffDistance and + /// logical_audio::ValidateVolumeLevel before calling this method in order to + /// prevent undefined behavior. + /// \param[in] _playing Whether the audio source is playing or not. + /// \param[in] _attenuationFunc The source's attenuation function. + /// \param[in] _attenuationShape The source's attenuation shape. + /// \param[in] _sourceEmissionVolume The source's emission volume level. + /// \param[in] _innerRadius The source's inner radius, which should be >= 0. + /// \param[in] _falloffDistance The source's falloffDistance, which should be + /// greater than _innerRadius. + /// \param[in] _sourcePose The source's pose. + /// \param[in] _targetPose The pose where the volume level should be + /// calculated. + /// \return The volume level at this location. + /// If the attenuation function or shape is undefined, -1.0 is returned. + /// If the source is not playing, 0.0 is returned. + double computeVolume(bool _playing, + AttenuationFunction _attenuationFunc, + AttenuationShape _attenuationShape, + double _sourceEmissionVolume, + double _innerRadius, + double _falloffDistance, + const ignition::math::Pose3d &_sourcePose, + const ignition::math::Pose3d &_targetPose); + + /// \brief Set the attenuation function that matches the defined string. + /// The string is not case sensitive, and must match the spelling + /// of the values in AttenuationFunction. If the spelling does not match, + /// the attenuation function is set as AttenuationFunction::Undefined. + /// + /// \em Example: to set the attenuation function to + /// AttenuationFunction::Linear, the following are examples of valid + /// strings: "linear", "Linear", "LINEAR" + /// + /// \param[out] _attenuationFunc A reference to the variable that stores + /// the calculated attenuation function. + /// \param[in] _str A string that should map to a value in + /// AttenuationFunction. + void setAttenuationFunction(AttenuationFunction &_attenuationFunc, + std::string _str); + + /// \brief Set the attenuation shape that matches the defined string. + /// The string is not case sensitive, and must match the spelling + /// of the values in AttenuationShape. If the spelling does not match, + /// the attenuation shape is set as AttenuationShape::Undefined. + /// + /// \em Example: to set the attenuation shape to AttenuationShape::Sphere, + /// the following are examples of valid strings: "sphere", "Sphere", "SPHERE" + /// + /// \param[out] _attenuationShape A reference to the variable that stores the + /// calculated attenuation shape. + /// \param[in] _str A string that should map to a value in + /// AttenuationShape. + void setAttenuationShape(AttenuationShape &_attenuationShape, + std::string _str); + + /// \brief Validate the inner radius and falloff distance for an audio source. + /// \param[in,out] _innerRadius The inner radius to set for the source. + /// This value must be > 0. + /// If the value of this parameter is < 0, the source's inner radius + /// will be set to 0. + /// \param[in,out] _falloffDistance The falloff distance to set for the + /// source. This value must be greater than _innerRadius. + /// If _falloffDistance < _innerRadius, _falloffDistance will be set to + /// _innerRadius + 1 (assuming that _innerRadius is valid). + void validateInnerRadiusAndFalloffDistance(double &_innerRadius, + double &_falloffDistance); + + /// \brief Validate a source's emission volume level. + /// \param[in,out] _volumeLevel The volume the source should play at. + /// This parameter is checked (and possibly clipped) to ensure that it falls + /// between 0.0 (0% volume) and 1.0 (100% volume). + void validateVolumeLevel(double &_volumeLevel); +} +} +} +} + +#endif diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc new file mode 100644 index 0000000000..33b9310b2b --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -0,0 +1,528 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "LogicalAudioSensorPlugin.hh" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "LogicalAudio.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::LogicalAudioSensorPluginPrivate +{ + /// \brief Creates an audio source with attributes specified in an SDF file. + /// \param[in] _elem A pointer to the source element in the SDF file. + /// \param[in] _parent The source element's parent entity. + /// \param[in] _ecm The simulation's EntityComponentManager. + /// \param[in] _sdfEntityCreator An SdfEntityCreator. + /// \param[in] _ids A list of audio source IDs that are connected to _parent. + public: void CreateAudioSource(const sdf::ElementPtr &_elem, + const Entity &_parent, + EntityComponentManager &_ecm, + SdfEntityCreator &_sdfEntityCreator, + std::unordered_set &_ids); + + /// \brief Creates a microphone with attributes specified in an SDF file. + /// \param[in] _elem A pointer to the microphone element in the SDF file. + /// \param[in] _parent The microphone element's parent entity. + /// \param[in] _ecm The simulation's EntityComponentManager. + /// \param[in] _sdfEntityCreator An SdfEntityCreator. + /// \param[in] _ids A list of microphone IDs that are connected to _parent. + public: void CreateMicrophone(const sdf::ElementPtr &_elem, + const Entity &_parent, + EntityComponentManager &_ecm, + SdfEntityCreator &_sdfEntityCreator, + std::unordered_set &_ids); + + /// \brief Checks if a source has exceeded its play duration. + /// \param[in] _simTimeInfo Information about the current simulation time. + /// \param[in] _sourcePlayInfo The source's playing information. + /// \returns true if the source's play duration has been exceeded, + /// false otherwise + public: bool DurationExceeded(const UpdateInfo &_simTimeInfo, + const logical_audio::SourcePlayInfo &_sourcePlayInfo); + + /// \brief Node used to create publishers and services + public: ignition::transport::Node node; + + /// \brief A flag used to initialize a source's playing information + /// before starting simulation. + public: bool firstTime{true}; + + /// \brief A list of source entities for a specific parent entity + /// (an entity can have multiple sources attached to it). + /// The value is a pair of booleans that indicate if the source + /// in the corresponding key should be played/stopped because of a service + /// call. The first element in the pair indicates if the play source + /// service was called, and the second element in the pair indicates if + /// the stop source service was called. + public: std::unordered_map> sourceEntities; + + /// \brief A list of microphone entities for a specific parent entity + /// (an entity can have multiple microphones attached to it). + /// The value is the microphone's detection publisher. + public: std::unordered_map micEntities; + + /// \brief A mutex used to ensure that the play source service call does + /// not interfere with the source's state in the PreUpdate step. + public: std::mutex playSourceMutex; + + /// \brief A mutex used to ensure that the stop source service call does + /// not interfere with the source's state in the PreUpdate step. + public: std::mutex stopSourceMutex; +}; + +////////////////////////////////////////////////// +LogicalAudioSensorPlugin::LogicalAudioSensorPlugin() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +LogicalAudioSensorPlugin::~LogicalAudioSensorPlugin() +{ +} + +////////////////////////////////////////////////// +void LogicalAudioSensorPlugin::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + const std::string kSource = "source"; + const std::string kMicrophone = "microphone"; + + SdfEntityCreator sdfEntityCreator(_ecm, _eventMgr); + + const auto sdfClone = _sdf->Clone(); + + if (sdfClone->HasElement(kSource)) + { + std::unordered_set allIDs; + for (auto sourceElem = sdfClone->GetElement(kSource); sourceElem; + sourceElem = sourceElem->GetNextElement(kSource)) + { + this->dataPtr->CreateAudioSource(sourceElem, _entity, _ecm, + sdfEntityCreator, allIDs); + } + } + + if (sdfClone->HasElement(kMicrophone)) + { + std::unordered_set allIDs; + for (auto micElem = sdfClone->GetElement(kMicrophone); micElem; + micElem = micElem->GetNextElement(kMicrophone)) + { + this->dataPtr->CreateMicrophone(micElem, _entity, _ecm, sdfEntityCreator, + allIDs); + } + } +} + +////////////////////////////////////////////////// +void LogicalAudioSensorPlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + for (auto & [entity, serviceFlags] : this->dataPtr->sourceEntities) + { + auto& playInfo = _ecm.Component( + entity)->Data(); + + // configure the source's play information before starting the simulation + if (this->dataPtr->firstTime) + playInfo.startTime = _info.simTime; + + // start playing a source if the play source service was called + std::unique_lock play_lock(this->dataPtr->playSourceMutex); + if (serviceFlags.first) + { + // only reset the source's play start time if it isn't playing already + // (calling the play service on a source that's already playing does + // nothing) + if (!playInfo.playing) + playInfo.startTime = _info.simTime; + + playInfo.playing = true; + serviceFlags.first = false; + } + play_lock.unlock(); + + // stop playing a source if the stop source service was called + std::unique_lock stop_lock(this->dataPtr->stopSourceMutex); + if (serviceFlags.second) + { + playInfo.playing = false; + serviceFlags.second = false; + } + stop_lock.unlock(); + + // stop playing a source if the play duration has been exceeded + if (this->dataPtr->DurationExceeded(_info, playInfo)) + playInfo.playing = false; + } + + this->dataPtr->firstTime = false; +} + +////////////////////////////////////////////////// +void LogicalAudioSensorPlugin::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + // get the current sim time so that it can be placed in the header + // of microphone detection messages + const auto simSeconds = + std::chrono::duration_cast(_info.simTime); + const auto simNanoseconds = + std::chrono::duration_cast(_info.simTime); + const auto nanosecondOffset = (simNanoseconds - simSeconds).count(); + + for (auto & [micEntity, detectionPub] : this->dataPtr->micEntities) + { + const auto micPose = worldPose(micEntity, _ecm); + const auto micInfo = _ecm.Component( + micEntity)->Data(); + + _ecm.Each( + [&, &publisher = detectionPub](const Entity &_entity, + const components::LogicalAudioSource *_source, + const components::LogicalAudioSourcePlayInfo *_playInfo) + { + const auto sourcePose = worldPose(_entity, _ecm); + const auto vol = logical_audio::computeVolume( + _playInfo->Data().playing, + _source->Data().attFunc, + _source->Data().attShape, + _source->Data().emissionVolume, + _source->Data().innerRadius, + _source->Data().falloffDistance, + sourcePose, + micPose); + + if (logical_audio::detect(vol, micInfo.volumeDetectionThreshold)) + { + // publish the source that the microphone heard, along with the + // volume level the microphone detected. The detected source's + // ID is embedded in the message's header + ignition::msgs::Double msg; + auto header = msg.mutable_header(); + auto timeStamp = header->mutable_stamp(); + timeStamp->set_sec(simSeconds.count()); + timeStamp->set_nsec(nanosecondOffset); + auto headerData = header->add_data(); + headerData->set_key(scopedName(_entity, _ecm)); + msg.set_data(vol); + + publisher.Publish(msg); + } + + return true; + }); + } +} + +////////////////////////////////////////////////// +void LogicalAudioSensorPluginPrivate::CreateAudioSource( + const sdf::ElementPtr &_elem, + const Entity &_parent, + EntityComponentManager &_ecm, + SdfEntityCreator &_sdfEntityCreator, + std::unordered_set &_ids) +{ + static const std::string kSourceSkipMsg = + "Skipping the creation of this source.\n"; + + if (!_elem->HasElement("id")) + { + ignerr << "Audio source is missing an id. " << kSourceSkipMsg; + return; + } + const auto id = _elem->Get("id"); + + // make sure no other sources exist with the same ID in the parent + if (_ids.find(id) != _ids.end()) + { + ignerr << "The specified source ID of " << id << " already exists for " + << "another source in entity " << _parent << ". " << kSourceSkipMsg; + return; + } + _ids.insert(id); + + ignition::math::Pose3d pose; + if (!_elem->HasElement("pose")) + { + ignwarn << "Audio source is missing a pose. " + << "{0.0, 0.0, 0.0, 0.0, 0.0, 0.0} will be used.\n"; + pose = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + } + else + { + pose = _elem->Get("pose"); + } + + if (!_elem->HasElement("attenuation_function")) + { + ignerr << "Audio source has no attenuation function. " << kSourceSkipMsg; + return; + } + const auto attenuationFunc = _elem->Get("attenuation_function"); + + if (!_elem->HasElement("attenuation_shape")) + { + ignerr << "Audio source has no attenuation shape. " << kSourceSkipMsg; + return; + } + const auto attenuationShape = _elem->Get("attenuation_shape"); + + if (!_elem->HasElement("inner_radius")) + { + ignerr << "Audio source has no inner radius. " << kSourceSkipMsg; + return; + } + const auto innerRadius = _elem->Get("inner_radius"); + + if (!_elem->HasElement("falloff_distance")) + { + ignerr << "Audio source is missing a falloff distance. " << kSourceSkipMsg; + return; + } + const auto falloffDistance = _elem->Get("falloff_distance"); + + if (!_elem->HasElement("volume_level")) + { + ignerr << "Audio source is missing a volume level. " << kSourceSkipMsg; + return; + } + const auto volumeLevel = _elem->Get("volume_level"); + + if (!_elem->HasElement("playing")) + { + ignerr << "Audio source is missing the playing attribute. " + << kSourceSkipMsg; + return; + } + const auto playing = _elem->Get("playing"); + + if (!_elem->HasElement("play_duration")) + { + ignerr << "Audio source is missing the play duration. " << kSourceSkipMsg; + return; + } + const auto playDuration = _elem->Get("play_duration"); + + // create an audio source entity + auto entity = _ecm.CreateEntity(); + if (entity == kNullEntity) + { + ignerr << "Failed to create a logical audio source entity. " + << kSourceSkipMsg; + return; + } + _sdfEntityCreator.SetParent(entity, _parent); + _ecm.CreateComponent(entity, + components::Name("source_" + std::to_string(id))); + _ecm.CreateComponent(entity, components::Sensor()); + + // save the audio source properties as a component + logical_audio::Source source; + source.id = id; + logical_audio::setAttenuationFunction(source.attFunc, attenuationFunc); + logical_audio::setAttenuationShape(source.attShape, attenuationShape); + source.innerRadius = innerRadius; + source.falloffDistance = falloffDistance; + logical_audio::validateInnerRadiusAndFalloffDistance( + source.innerRadius, + source.falloffDistance); + source.emissionVolume = volumeLevel; + logical_audio::validateVolumeLevel(source.emissionVolume); + _ecm.CreateComponent(entity, components::LogicalAudioSource(source)); + + // save the source's pose as a component + _ecm.CreateComponent(entity, + components::Pose(pose)); + + // save the source's playing information as a component + logical_audio::SourcePlayInfo playInfo; + playInfo.playing = playing; + playInfo.playDuration = std::chrono::seconds(playDuration); + _ecm.CreateComponent(entity, + components::LogicalAudioSourcePlayInfo(playInfo)); + + // create service callbacks that allow this source to be played/stopped + std::function playSrvCb = + [this, entity](ignition::msgs::Boolean &_resp) + { + std::lock_guard lock(this->playSourceMutex); + this->sourceEntities[entity].first = true; + _resp.set_data(true); + return true; + }; + std::function stopSrvCb = + [this, entity](ignition::msgs::Boolean &_resp) + { + std::lock_guard lock(this->stopSourceMutex); + this->sourceEntities[entity].second = true; + _resp.set_data(true); + return true; + }; + + // create services for this source + const auto full_name = scopedName(entity, _ecm); + if (!this->node.Advertise(full_name + "/play", playSrvCb)) + { + ignerr << "Error advertising the play source service for source " + << id << " in entity " << _parent << ". " << kSourceSkipMsg; + return; + } + if (!this->node.Advertise(full_name + "/stop", stopSrvCb)) + { + ignerr << "Error advertising the stop source service for source " + << id << " in entity " << _parent << ". " << kSourceSkipMsg; + return; + } + + this->sourceEntities.insert({entity, {false, false}}); +} + +////////////////////////////////////////////////// +void LogicalAudioSensorPluginPrivate::CreateMicrophone( + const sdf::ElementPtr &_elem, + const Entity &_parent, + EntityComponentManager &_ecm, + SdfEntityCreator &_sdfEntityCreator, + std::unordered_set &_ids) +{ + static const std::string kMicSkipMsg = + "Skipping the creation of this microphone.\n"; + + if (!_elem->HasElement("id")) + { + ignerr << "Microphone is missing an id. " << kMicSkipMsg; + return; + } + const auto id = _elem->Get("id"); + + // make sure no other microphones exist with the same ID in the parent + if (_ids.find(id) != _ids.end()) + { + ignerr << "The specified microphone ID of " << id << " already exists for " + << "another microphone in entity " << _parent << ". " << kMicSkipMsg; + return; + } + _ids.insert(id); + + ignition::math::Pose3d pose; + if (!_elem->HasElement("pose")) + { + ignwarn << "Microphone is missing a pose. " + << "{0.0, 0.0, 0.0, 0.0, 0.0, 0.0} will be used.\n"; + pose = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + } + else + { + pose = _elem->Get("pose"); + } + + double volumeDetectionThreshold; + if (!_elem->HasElement("volume_threshold")) + { + ignwarn << "Microphone is missing a volume threshold. 0.0 will be used.\n"; + volumeDetectionThreshold = 0.0; + } + else + { + volumeDetectionThreshold = _elem->Get("volume_threshold"); + } + + // create a microphone entity + auto entity = _ecm.CreateEntity(); + if (entity == kNullEntity) + { + ignerr << "Failed to create a logical audio microphone entity. " + << kMicSkipMsg; + return; + } + _sdfEntityCreator.SetParent(entity, _parent); + _ecm.CreateComponent(entity, + components::Name("mic_" + std::to_string(id))); + _ecm.CreateComponent(entity, components::Sensor()); + + // save the microphone properties as a component + logical_audio::Microphone microphone; + microphone.id = id; + microphone.volumeDetectionThreshold = volumeDetectionThreshold; + _ecm.CreateComponent(entity, components::LogicalMicrophone(microphone)); + + // save the microphone's pose as a component + _ecm.CreateComponent(entity, + components::Pose(pose)); + + // create the detection publisher for this microphone + auto pub = this->node.Advertise( + scopedName(entity, _ecm) + "/detection"); + if (!pub) + { + ignerr << "Error creating a detection publisher for microphone " + << id << " in entity " << _parent << ". " << kMicSkipMsg; + return; + } + + this->micEntities.insert({entity, pub}); +} + +////////////////////////////////////////////////// +bool LogicalAudioSensorPluginPrivate::DurationExceeded( + const UpdateInfo &_simTimeInfo, + const logical_audio::SourcePlayInfo &_sourcePlayInfo) +{ + auto currDuration = _simTimeInfo.simTime - _sourcePlayInfo.startTime; + + // make sure the source doesn't have an infinite play duration + return (_sourcePlayInfo.playDuration.count() > 0 ) && + (currDuration > _sourcePlayInfo.playDuration); +} + +IGNITION_ADD_PLUGIN(LogicalAudioSensorPlugin, + ignition::gazebo::System, + LogicalAudioSensorPlugin::ISystemConfigure, + LogicalAudioSensorPlugin::ISystemPreUpdate, + LogicalAudioSensorPlugin::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(LogicalAudioSensorPlugin, + "ignition::gazebo::systems::LogicalAudioSensorPlugin") diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh new file mode 100644 index 0000000000..5f0d261a1c --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh @@ -0,0 +1,165 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_LOGICAL_AUDIO_SENSOR_PLUGIN_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LOGICAL_AUDIO_SENSOR_PLUGIN_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward Declaration + class LogicalAudioSensorPluginPrivate; + + /// \brief A plugin for logical audio detection. + /// + /// Each tag can accept multiple sensors (sound sources + /// and/or microphones). + /// After each simulation step, microphones check if audio + /// was detected by any sources in the world. + /// No audio is actually played to an audio device + /// such as speakers. This plugin is meant to check if audio + /// could theoretically be heard at a certain location or not. + /// + /// Secifying an audio source via SDF is done as follows: + /// + /// A new audio source in the environment, which has the + /// following child elements: + /// * The source ID, which must be unique and an integer >= 0. + /// An ID < 0 results in undefined behavior. + /// The ID must be unique within the scope of all other source IDs + /// in the plugin's parent element - so, if a source was created in a + /// with an ID of 1, no other sources in the same model can have + /// an ID of 1 (however, sources that belong to other models can have an + /// ID of 1). + /// * The pose, expressed as "x y z roll pitch yaw". + /// Each pose coordinate must be separated by whitespace. + /// The pose is defined relative to the plugin's parent element. + /// So, if the plugin is used inside of a tag, then the + /// source's is relative to the model's pose. + /// If the plugin is used inside of a tag, then the source's + /// is relative to the world (i.e., specifies an + /// absolute pose). + /// If no pose is specified, {0, 0, 0, 0, 0, 0} will be used. + /// * The attenuation function. + /// See logical_audio::AttenuationFunction for a list of valid + /// attenuation functions, and logical_audio::SetAttenuationFunction + /// for how to specify an attenuation function in SDF. + /// * The attenuation shape. + /// See logical_audio::AttenuationShape for a list of valid + /// attenuation shapes, and logical_audio::SetAttenuationShape for how + /// to specify an attenuation shape in SDF. + /// * The inner radius of the attenuation shape. + /// This value must be >= 0.0. The volume of the source will be + /// at locations that have a distance <= inner + /// radius from the source. + /// * The falloff distance. This value must be greater + /// than the value of the source's . This defines the + /// distance from the audio source where the volume becomes 0. + /// * The volume level emitted from the source. This must + /// be a value between 0.0 and 1.0 (representing 0% to 100%). + /// * Whether the source should play immediately or not. + /// Use true to initiate audio immediately, and false otherwise. + /// * The duration (in seconds) audio is played from the + /// source. This value must be an integer >= 0. + /// A value of 0 means that the source will play for an infinite amount + /// of time. + /// + /// Specifying a microphone via SDF is done as follows: + /// + /// A new microphone in the environment, + /// which has the following child elements: + /// * The microphone ID, which must be unique and an integer >= 0. + /// An ID < 0 results in undefined behavior. + /// The ID must be unique within the scope of all other microphone IDs + /// in the plugin's parent element - so, if a microphone was created in + /// a with an ID of 1, no other microphones in the same model + /// can have an ID of 1 (however, microphones that belong to other + /// models can have an ID of 1). + /// * The pose, expressed as "x y z roll pitch yaw". + /// Each pose coordinate must be separated by whitespace. + /// The pose is defined relative to the plugin's parent element. + /// So, if the plugin is used inside of a tag, then the + /// source's is relative to the model's pose. + /// If the plugin is used inside of a tag, then the source's + /// is relative to the world (i.e., specifies an + /// absolute pose). + /// If no pose is specified, {0, 0, 0, 0, 0, 0} will be used. + /// * The minimum volume level the microphone + /// can detect. This must be a value between 0.0 and 1.0 + /// (representing 0% to 100%). + /// If no volume threshold is specified, 0.0 will be used. + /// + /// Sources can be started and stopped via Ignition service calls. + /// If a source is successfully created, the following services will be + /// created for the source ( is the scoped name for the source - see + /// ignition::gazebo::scopedName for more details - and is the value + /// specified in the source's tag from the SDF): + /// * /source_/play + /// * Starts playing the source with the specified ID. + /// If the source is already playing, nothing happens. + /// * /source_/stop + /// * Stops playing the source with the specified ID. + /// If the source is already stopped, nothing happens. + /// + /// Microphone detection information can be retrieved via Ignition topics. + /// Whenever a microphone detects a source, it publishes a message to the + /// /mic_/detection topic, where is the scoped name + /// for the microphone - see ignition::gazebo::scopedName for more details - + /// and is the value specified in the microphone's tag from the SDF. + class IGNITION_GAZEBO_VISIBLE LogicalAudioSensorPlugin : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: LogicalAudioSensorPlugin(); + + /// \brief Destructor + public: ~LogicalAudioSensorPlugin() override; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} + +#endif diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudio_TEST.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudio_TEST.cc new file mode 100644 index 0000000000..b45d7d45c8 --- /dev/null +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudio_TEST.cc @@ -0,0 +1,299 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include "LogicalAudio.hh" + +namespace logical_audio = ignition::gazebo::logical_audio; + +using AttenuationFunction = logical_audio::AttenuationFunction; +using AttenuationShape = logical_audio::AttenuationShape; + +////////////////////////////////////////////////// +TEST(LogicalAudioTest, Detect) +{ + // expect a detection if the volume level is >= the detection threshold + // (as long as the volume level is > 0.0) + EXPECT_TRUE(logical_audio::detect(1.0, 0.0)); + EXPECT_TRUE(logical_audio::detect(0.1, 0.0)); + EXPECT_TRUE(logical_audio::detect(0.7, 0.5)); + EXPECT_TRUE(logical_audio::detect(1.0, 1.0)); + + // expect no detection if the volume level is < the detection threshold + EXPECT_FALSE(logical_audio::detect(0.2, 0.5)); + EXPECT_FALSE(logical_audio::detect(0.7, 0.9)); + EXPECT_FALSE(logical_audio::detect(0.9, 1.0)); + + // make sure no detections occur if the volume level is 0 + EXPECT_FALSE(logical_audio::detect(0.0, 0.0)); + EXPECT_FALSE(logical_audio::detect(0.0, 0.5)); + EXPECT_FALSE(logical_audio::detect(0.0, 1.0)); +} + +////////////////////////////////////////////////// +TEST(LogicalAudioTest, ComputeVolume) +{ + // make sure computed volume is the source's emission volume inside of the + // inner radius + EXPECT_DOUBLE_EQ(1.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 1.0, 5.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(1.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 1.0, 5.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.5, 0.5, 0.5, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.3, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.3, 4.0, 5.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 2.0, 0.5, 1.0, 6.0, 9.0})); + + // check volume at the inner radius + EXPECT_DOUBLE_EQ(1.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 1.0, 5.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {1.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.6, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.6, 10.0, 25.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 10.0, 0.5, 2.0, -0.1})); + + // check volume between the inner radius and falloff distance + EXPECT_DOUBLE_EQ(0.5, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 1.0, 11.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {6.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.1, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 1.0, 11.0, + {0.0, 0.0, 0.0, -0.1, 0.6, 2.0}, + {10.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.5, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 0.0, 10.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {5.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.75, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 0.0, 10.0, + {0.0, 0.0, 0.0, 0.8, 0.2, 5.0}, + {0.0, 2.5, 0.0, 1.0, 0.3, 9.0})); + EXPECT_DOUBLE_EQ(0.125, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.5, 0.0, 10.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 7.5, -1.0, -0.3, 7.2})); + + // check volume at the falloff distance (make sure it is 0) + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 0.0, 10.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {10.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.3, 2.0, 5.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {3.0, 4.0, 0.0, 1.0, -0.5, 0.8})); + + // check volume past the falloff distance (make sure it is 0) + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 5.0, 10.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {10.0, 10.0, 10.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.4, 5.0, 8.0, + {0.0, 0.0, 0.0, 1.0, 2.0, 3.0}, + {210.0, 15.0, 0.0, 1.0, 5.0, -0.7})); + + // make sure computed volume is 0 if source isn't playing + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(false, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 1.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(false, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.5, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(false, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {1.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(false, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.8, 0.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {50.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(false, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.8, 0.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {500.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + + // make sure undefined attenuation functions and shapes are handled + EXPECT_DOUBLE_EQ(-1.0, + logical_audio::computeVolume(true, AttenuationFunction::UNDEFINED, + AttenuationShape::SPHERE, 1.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(-1.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::UNDEFINED, 1.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 5.0, 0.0, 0.0, 0.0})); + + // make sure computed volume is always 0 if source emission volume is 0 + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + EXPECT_DOUBLE_EQ(0.0, + logical_audio::computeVolume(true, AttenuationFunction::LINEAR, + AttenuationShape::SPHERE, 0.0, 10.0, 100.0, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {20.0, 20.0, 20.0, 0.0, 0.0, 0.0})); +} + +////////////////////////////////////////////////// +TEST(LogicalAudioTest, AttenuationSetters) +{ + // test valid attenuation function strings + AttenuationFunction func = AttenuationFunction::UNDEFINED; + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + logical_audio::setAttenuationFunction(func, "linear"); + EXPECT_EQ(AttenuationFunction::LINEAR, func); + func = AttenuationFunction::UNDEFINED; + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + logical_audio::setAttenuationFunction(func, "LINEAR"); + EXPECT_EQ(AttenuationFunction::LINEAR, func); + func = AttenuationFunction::UNDEFINED; + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + logical_audio::setAttenuationFunction(func, "LiNeaR"); + EXPECT_EQ(AttenuationFunction::LINEAR, func); + + // test valid attenuation shape strings + AttenuationShape shape = AttenuationShape::UNDEFINED; + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); + logical_audio::setAttenuationShape(shape, "sphere"); + EXPECT_EQ(AttenuationShape::SPHERE, shape); + shape = AttenuationShape::UNDEFINED; + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); + logical_audio::setAttenuationShape(shape, "SPHERE"); + EXPECT_EQ(AttenuationShape::SPHERE, shape); + shape = AttenuationShape::UNDEFINED; + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); + logical_audio::setAttenuationShape(shape, "sPHerE"); + EXPECT_EQ(AttenuationShape::SPHERE, shape); + + // test invalid attenuation function strings + func = AttenuationFunction::LINEAR; + EXPECT_EQ(AttenuationFunction::LINEAR, func); + logical_audio::setAttenuationFunction(func, "linear "); + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + func = AttenuationFunction::LINEAR; + EXPECT_EQ(AttenuationFunction::LINEAR, func); + logical_audio::setAttenuationFunction(func, " LINEAR"); + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + func = AttenuationFunction::LINEAR; + EXPECT_EQ(AttenuationFunction::LINEAR, func); + logical_audio::setAttenuationFunction(func, "some random string"); + EXPECT_EQ(AttenuationFunction::UNDEFINED, func); + + // test invalid attenuation shape strings + shape = AttenuationShape::SPHERE; + EXPECT_EQ(AttenuationShape::SPHERE, shape); + logical_audio::setAttenuationShape(shape, " sphere "); + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); + shape = AttenuationShape::SPHERE; + EXPECT_EQ(AttenuationShape::SPHERE, shape); + logical_audio::setAttenuationShape(shape, "SPHERE "); + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); + shape = AttenuationShape::SPHERE; + EXPECT_EQ(AttenuationShape::SPHERE, shape); + logical_audio::setAttenuationShape(shape, "hello, world!"); + EXPECT_EQ(AttenuationShape::UNDEFINED, shape); +} + +////////////////////////////////////////////////// +TEST(LogicalAudioTest, SourceComponentValidators) +{ + // make sure already valid inner radius and falloff distance aren't changed + double innerRadius = 1.0; + double falloffDistance = 5.0; + logical_audio::validateInnerRadiusAndFalloffDistance(innerRadius, + falloffDistance); + EXPECT_DOUBLE_EQ(1.0, innerRadius); + EXPECT_DOUBLE_EQ(5.0, falloffDistance); + innerRadius = 0.0; + falloffDistance = 0.5; + logical_audio::validateInnerRadiusAndFalloffDistance(innerRadius, + falloffDistance); + EXPECT_DOUBLE_EQ(0.0, innerRadius); + EXPECT_DOUBLE_EQ(0.5, falloffDistance); + + // make sure invalid inner radius and falloff distance are changed + innerRadius = 1.0; + falloffDistance = 0.5; + logical_audio::validateInnerRadiusAndFalloffDistance(innerRadius, + falloffDistance); + EXPECT_DOUBLE_EQ(1.0, innerRadius); + EXPECT_DOUBLE_EQ(2.0, falloffDistance); + innerRadius = -1.0; + falloffDistance = 0.5; + logical_audio::validateInnerRadiusAndFalloffDistance(innerRadius, + falloffDistance); + EXPECT_DOUBLE_EQ(0.0, innerRadius); + EXPECT_DOUBLE_EQ(0.5, falloffDistance); + innerRadius = -1.0; + falloffDistance = -0.5; + logical_audio::validateInnerRadiusAndFalloffDistance(innerRadius, + falloffDistance); + EXPECT_DOUBLE_EQ(0.0, innerRadius); + EXPECT_DOUBLE_EQ(1.0, falloffDistance); + + // make sure valid volume level isn't clipped + double vol = 0.5; + logical_audio::validateVolumeLevel(vol); + EXPECT_DOUBLE_EQ(0.5, vol); + vol = 0.0; + logical_audio::validateVolumeLevel(vol); + EXPECT_DOUBLE_EQ(0.0, vol); + vol = 1.0; + logical_audio::validateVolumeLevel(vol); + EXPECT_DOUBLE_EQ(1.0, vol); + + // make sure invalid volume level is clipped + vol = 2.0; + logical_audio::validateVolumeLevel(vol); + EXPECT_DOUBLE_EQ(1.0, vol); + vol = -5.0; + logical_audio::validateVolumeLevel(vol); + EXPECT_DOUBLE_EQ(0.0, vol); +} diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 1e0749772c..8885973829 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 3cf1fccada..84b5276e31 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -702,7 +702,6 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) << std::endl; return true; } - // TODO(anyone) Don't load models unless they have collisions // Check if parent world / model exists diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index e466d4fb7d..576a081bc0 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -75,6 +76,10 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \return True if successful. public: bool StateService(ignition::msgs::SerializedStepMap &_res); + /// \brief Callback for state service - non blocking. + /// \param[out] _res Response containing the last available full state. + public: void StateAsyncService(const ignition::msgs::StringMsg &_req); + /// \brief Updates the scene graph when entities are added /// \param[in] _manager The entity component manager public: void SceneGraphAddEntities(const EntityComponentManager &_manager); @@ -190,6 +195,9 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief Flag used to indicate if the state service was called. public: bool stateServiceRequest{false}; + + /// \brief A list of async state requests + public: std::unordered_set stateRequests; }; ////////////////////////////////////////////////// @@ -270,6 +278,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->stateServiceRequest || shouldPublish) { + std::unique_lock lock(this->dataPtr->stateMutex); this->dataPtr->stepMsg.Clear(); set(this->dataPtr->stepMsg.mutable_stats(), _info); @@ -294,6 +303,16 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, this->dataPtr->stateCv.notify_all(); } + // process async state requests + if (!this->dataPtr->stateRequests.empty()) + { + for (const auto &reqSrv : this->dataPtr->stateRequests) + { + this->dataPtr->node->Request(reqSrv, this->dataPtr->stepMsg); + } + this->dataPtr->stateRequests.clear(); + } + // Poses periodically + change events // TODO(louise) Send changed state periodically instead, once it reflects // changed components @@ -448,6 +467,8 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) << graphService << "]" << std::endl; // State service + // Note: GuiRunner used to call this service but it is now using the async + // version (state_async) std::string stateService{"state"}; this->node->Advertise(stateService, &SceneBroadcasterPrivate::StateService, @@ -456,6 +477,15 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) ignmsg << "Serving full state on [" << opts.NameSpace() << "/" << stateService << "]" << std::endl; + // Async State service + std::string stateAsyncService{"state_async"}; + + this->node->Advertise(stateAsyncService, + &SceneBroadcasterPrivate::StateAsyncService, this); + + ignmsg << "Serving full state (async) on [" << opts.NameSpace() << "/" + << stateAsyncService << "]" << std::endl; + // Scene info topic std::string sceneTopic{"/world/" + _worldName + "/scene/info"}; @@ -523,6 +553,15 @@ bool SceneBroadcasterPrivate::SceneInfoService(ignition::msgs::Scene &_res) return true; } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::StateAsyncService( + const ignition::msgs::StringMsg &_req) +{ + std::unique_lock lock(this->stateMutex); + this->stateServiceRequest = true; + this->stateRequests.insert(_req.data()); +} + ////////////////////////////////////////////////// bool SceneBroadcasterPrivate::StateService( ignition::msgs::SerializedStepMap &_res) @@ -531,6 +570,7 @@ bool SceneBroadcasterPrivate::StateService( // Lock and wait for an iteration to be run and fill the state std::unique_lock lock(this->stateMutex); + this->stateServiceRequest = true; auto success = this->stateCv.wait_for(lock, 5s, [&] { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 74b5b36bd1..d973d94a60 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index b431b86c7d..d6e07f48e7 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index bcdd636f18..bcb305e1b6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -25,6 +25,7 @@ set(tests level_manager_runtime_performers.cc link.cc logical_camera_system.cc + logical_audio_sensor_plugin.cc magnetometer_system.cc model.cc multicopter.cc diff --git a/test/integration/components.cc b/test/integration/components.cc index 00649aa031..57bc96a2ba 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -17,6 +17,8 @@ #include +#include + #include #include #include @@ -53,6 +55,7 @@ #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/LogicalAudio.hh" #include "ignition/gazebo/components/Magnetometer.hh" #include "ignition/gazebo/components/Material.hh" #include "ignition/gazebo/components/Model.hh" @@ -857,6 +860,112 @@ TEST_F(ComponentsTest, Link) comp3.Deserialize(istr); } +////////////////////////////////////////////////// +TEST_F(ComponentsTest, LogicalAudioSource) +{ + logical_audio::Source source1; + source1.id = 0; + source1.attFunc = logical_audio::AttenuationFunction::LINEAR; + source1.attShape = logical_audio::AttenuationShape::UNDEFINED; + source1.innerRadius = 0.25; + source1.falloffDistance = 5.0; + source1.emissionVolume = 1.0; + + logical_audio::Source source2; + source2.id = 1; + source2.attFunc = source1.attFunc; + source2.attShape = source1.attShape; + source2.innerRadius = source1.innerRadius; + source2.falloffDistance = source1.falloffDistance; + source2.emissionVolume = source1.emissionVolume; + + // create components + auto comp1 = components::LogicalAudioSource(source1); + auto comp2 = components::LogicalAudioSource(source2); + + // equality operators + EXPECT_NE(comp1, comp2); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("0 0 1 0.25 5 1", ostr.str()); + + std::istringstream istr(ostr.str()); + components::LogicalAudioSource comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1, comp3); +} + +///////////////////////////////////////////////// +TEST_F(ComponentsTest, LogicalAudioSourcePlayInfo) +{ + auto start = std::chrono::steady_clock::now(); + auto end = start + std::chrono::seconds(1); + + logical_audio::SourcePlayInfo playInfo1; + playInfo1.playing = true; + playInfo1.playDuration = std::chrono::seconds(1); + playInfo1.startTime = end - start; + + logical_audio::SourcePlayInfo playInfo2; + playInfo2.playing = false; + playInfo2.playDuration = std::chrono::seconds(5); + playInfo2.startTime = end - start; + + // create components + auto comp1 = components::LogicalAudioSourcePlayInfo(playInfo1); + auto comp2 = components::LogicalAudioSourcePlayInfo(playInfo2); + + // equality operators + EXPECT_NE(comp1, comp2); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("1 1 1000000000", ostr.str()); + + std::istringstream istr(ostr.str()); + components::LogicalAudioSourcePlayInfo comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1, comp3); +} + +///////////////////////////////////////////////// +TEST_F(ComponentsTest, LogicalMicrophone) +{ + logical_audio::Microphone mic1; + mic1.id = 0; + mic1.volumeDetectionThreshold = 0.5; + + logical_audio::Microphone mic2; + mic2.id = 1; + mic2.volumeDetectionThreshold = mic1.volumeDetectionThreshold; + + // create components + auto comp1 = components::LogicalMicrophone(mic1); + auto comp2 = components::LogicalMicrophone(mic2); + + // equality operators + EXPECT_NE(mic1, mic2); + EXPECT_FALSE(mic1 == mic2); + EXPECT_TRUE(mic1 != mic2); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("0 0.5", ostr.str()); + + std::istringstream istr; + components::LogicalMicrophone comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1, comp3); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, Magnetometer) { diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 7ebc78cabd..06dc2b62e7 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -1594,8 +1594,8 @@ TEST_F(LogSystemTest, LogResources) // Recorded models should exist EXPECT_GT(entryCount(recordPath), 2); EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".ignition", "fuel", "fuel.ignitionrobotics.org", "OpenRobotics", - "models", "X2 Config 1"))); + ".ignition", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + "models", "x2 config 1"))); // Remove artifacts. Recreate new directory this->RemoveLogsDir(); @@ -1629,8 +1629,8 @@ TEST_F(LogSystemTest, LogResources) EXPECT_GT(entryCount(recordPath), 1); #endif EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".ignition", "fuel", "fuel.ignitionrobotics.org", "OpenRobotics", - "models", "X2 Config 1"))); + ".ignition", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + "models", "x2 config 1"))); // Revert environment variable after test is done EXPECT_EQ(setenv(IGN_HOMEDIR, homeOrig.c_str(), 1), 0); diff --git a/test/integration/logical_audio_sensor_plugin.cc b/test/integration/logical_audio_sensor_plugin.cc new file mode 100644 index 0000000000..ddcbdf82f7 --- /dev/null +++ b/test/integration/logical_audio_sensor_plugin.cc @@ -0,0 +1,326 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/components/LogicalAudio.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/ServerConfig.hh" +#include "ignition/gazebo/Types.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test LogicalAudio system plugin +class LogicalAudioTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + +TEST_F(LogicalAudioTest, LogicalAudioDetections) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/logical_audio_sensor_plugin.sdf"; + serverConfig.SetSdfFile(sdfFile); + + // start server + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // helper variables for checking the validity of the ECM + const ignition::math::Pose3d sourcePose(0, 0, 0, 0, 0, 0); + const auto zeroSeconds = std::chrono::seconds(0); + const ignition::math::Pose3d micClosePose(0.5, 0, 0, 0, 0, 0); + const ignition::math::Pose3d micFarPose(0, 0, 0, 0, 0, 0); + std::chrono::steady_clock::duration sourceStartTime; + bool firstTime{true}; + + // flags that verify the ECM was checked for the source and microphones + bool checkedSource{false}; + bool checkedMicClose{false}; + bool checkedMicFar{false}; + + // make a test system and check the ECM for the source and microphones + test::Relay testSystem; + testSystem.OnPreUpdate([&](const UpdateInfo &_info, + EntityComponentManager &/*_ecm*/) + { + if (firstTime) + sourceStartTime = _info.simTime; + firstTime = false; + }); + testSystem.OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + // make sure the source is stored correctly in the ECM + _ecm.Each( + [&](const Entity &/*_entity*/, + const components::LogicalAudioSource *_source, + const components::LogicalAudioSourcePlayInfo *_playInfo, + const components::Pose *_pose) + { + EXPECT_EQ(_source->Data().id, 1u); + EXPECT_EQ(_source->Data().attFunc, + logical_audio::AttenuationFunction::LINEAR); + EXPECT_EQ(_source->Data().attShape, + logical_audio::AttenuationShape::SPHERE); + EXPECT_DOUBLE_EQ(_source->Data().innerRadius, 3.0); + EXPECT_DOUBLE_EQ(_source->Data().falloffDistance, 8.0); + EXPECT_DOUBLE_EQ(_source->Data().emissionVolume, 0.9); + + EXPECT_TRUE(_playInfo->Data().playing); + EXPECT_EQ(_playInfo->Data().playDuration, zeroSeconds); + EXPECT_EQ(_playInfo->Data().startTime, sourceStartTime); + + EXPECT_EQ(_pose->Data(), sourcePose); + + checkedSource = true; + return true; + }); + + // make sure the microphones are stored correctly in the ECM + _ecm.Each( + [&](const Entity &/*_entity*/, + const components::LogicalMicrophone *_mic, + const components::Pose *_pose) + { + if (_mic->Data().id == 2u) + { + EXPECT_EQ(_mic->Data().id, 2u); + EXPECT_DOUBLE_EQ(_mic->Data().volumeDetectionThreshold, 0.1); + EXPECT_EQ(_pose->Data(), micClosePose); + checkedMicClose = true; + } + else if (_mic->Data().id == 1u) + { + EXPECT_EQ(_mic->Data().id, 1u); + EXPECT_DOUBLE_EQ(_mic->Data().volumeDetectionThreshold, 0.0); + EXPECT_EQ(_pose->Data(), micFarPose); + checkedMicFar = true; + } + return true; + }); + }); + server.AddSystem(testSystem.systemPtr); + + // subscribe to the close microphone's detection topic + const std::string closeTopic = + "/model/mic_model_close/sensor/mic_2/detection"; + bool receivedClose{false}; + msgs::Double msg; + msg.Clear(); + std::function closeCb = + [&receivedClose, &msg](const msgs::Double &_msg) + { + // only need one message + if (receivedClose) + return; + + msg = _msg; + receivedClose = true; + }; + transport::Node node; + auto subscribedClose = node.Subscribe(closeTopic, closeCb); + EXPECT_TRUE(subscribedClose); + + // subscribe to the far microphone's detection topic + const std::string farTopic = "/model/mic_model_far/sensor/mic_1/detection"; + bool receivedFar{false}; + std::function farCb = + [&receivedFar](const msgs::Double &/*_msg*/) + { + receivedFar = true; + }; + auto subscribedFar = node.Subscribe(farTopic, farCb); + EXPECT_TRUE(subscribedFar); + + // make sure the microphone topics being subscribed to are being advertised + std::vector allTopics; + node.TopicList(allTopics); + bool closeTopicAdvertised{false}; + bool farTopicAdvertised{false}; + for (const auto & topic : allTopics) + { + if (topic == closeTopic) + closeTopicAdvertised = true; + else if (topic == farTopic) + farTopicAdvertised = true; + } + EXPECT_TRUE(closeTopicAdvertised); + EXPECT_TRUE(farTopicAdvertised); + + // make sure close microphone detection occurred, and that the far microphone + // didn't detect anything + server.Run(true, 100, false); + // (wait on ignition-transport for close detection message to be received. + // Don't exit when a close microphone detection is received because we want to + // make sure a far microphone detection is never received) + for (auto sleep = 0; sleep < 30; ++sleep) + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_FALSE(firstTime); + EXPECT_TRUE(checkedSource); + EXPECT_TRUE(checkedMicClose); + EXPECT_TRUE(checkedMicFar); + EXPECT_TRUE(receivedClose); + EXPECT_FALSE(receivedFar); + EXPECT_EQ(msg.header().data(0).key(), + "world/logical_audio_sensor/model/source_model/sensor/source_1"); +} + +TEST_F(LogicalAudioTest, LogicalAudioServices) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/logical_audio_sensor_plugin_services.sdf"; + serverConfig.SetSdfFile(sdfFile); + + // start server + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // services to test + const std::string playService = + "/model/model_not_playing/sensor/source_2/play"; + const std::string stopService = + "/model/model_playing/sensor/source_1/stop"; + const unsigned int timeout = 1000; + + bool firstTime{true}; + bool checkedSource1BeforeChange{false}; + bool checkedSource2BeforeChange{false}; + bool checkedSource1AfterChange{false}; + bool checkedSource2AfterChange{false}; + + transport::Node node; + + // make a test system to test logical audio source's play/stop services + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &/*_entity*/, + const components::LogicalAudioSource *_source, + const components::LogicalAudioSourcePlayInfo *_playInfo) + { + if (firstTime) + { + // used for service calls + msgs::Boolean response; + bool result; + + // check source playing state before service call + if (_source->Data().id == 1u) + { + EXPECT_TRUE(_playInfo->Data().playing); + checkedSource1BeforeChange = true; + + // call the stop service + auto executed = node.Request(stopService, timeout, response, + result); + EXPECT_TRUE(executed); + EXPECT_TRUE(response.data()); + EXPECT_TRUE(result); + } + else if (_source->Data().id == 2u) + { + EXPECT_FALSE(_playInfo->Data().playing); + checkedSource2BeforeChange = true; + + // call the play service + auto executed = node.Request(playService, timeout, response, + result); + EXPECT_TRUE(executed); + EXPECT_TRUE(response.data()); + EXPECT_TRUE(result); + } + } + else + { + // check source playing state after service call + if (_source->Data().id == 1u) + { + EXPECT_FALSE(_playInfo->Data().playing); + checkedSource1AfterChange = true; + } + else if (_source->Data().id == 2u) + { + EXPECT_TRUE(_playInfo->Data().playing); + checkedSource2AfterChange = true; + } + } + + return true; + }); + + firstTime = false; + }); + server.AddSystem(testSystem.systemPtr); + + // make sure the play/stop services exist + std::vector allServices; + node.ServiceList(allServices); + bool playServiceAdvertised{false}; + bool stopServiceAdvertised{false}; + for (const auto & service : allServices) + { + if (service == playService) + playServiceAdvertised = true; + else if (service == stopService) + stopServiceAdvertised = true; + } + EXPECT_TRUE(playServiceAdvertised); + EXPECT_TRUE(stopServiceAdvertised); + + // run the server to test the play/stop services + server.Run(true, 100, false); + EXPECT_FALSE(firstTime); + EXPECT_TRUE(checkedSource1BeforeChange); + EXPECT_TRUE(checkedSource2BeforeChange); + EXPECT_TRUE(checkedSource1AfterChange); + EXPECT_TRUE(checkedSource2AfterChange); +} diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index f8b3598c60..171cf8c8c1 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -640,7 +640,7 @@ TEST_F(PhysicsSystemFixture, ResetPositionComponent) double pos0 = 0.42; - /// cppcheck-suppress variableScope + // cppcheck-suppress variableScope bool firstRun = true; testSystem.OnPreUpdate( @@ -739,7 +739,7 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) double vel0 = 3.0; - /// cppcheck-suppress variableScope + // cppcheck-suppress variableScope bool firstRun = true; testSystem.OnPreUpdate( diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 22f47cc028..8caac85336 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -82,6 +82,8 @@ TEST_P(SceneBroadcasterTest, PoseInfo) unsigned int sleep{0u}; unsigned int maxSleep{10u}; + // cppcheck-suppress unmatchedSuppression + // cppcheck-suppress knownConditionTrueFalse while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); @@ -397,6 +399,13 @@ TEST_P(SceneBroadcasterTest, State) checkMsg(_res, 3); }; + // async state request with full state response + std::function cbAsync = + [&](const msgs::SerializedStepMap &_res) + { + checkMsg(_res, 16); + }; + // The request is blocking even though it's meant to be async, so we spin a // thread auto request = [&]() @@ -429,6 +438,26 @@ TEST_P(SceneBroadcasterTest, State) while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); EXPECT_TRUE(received); + + // test async state request + received = false; + std::string reqSrv = "/state_async_callback_test"; + node.Advertise(reqSrv, cbAsync); + + ignition::msgs::StringMsg req; + req.set_data(reqSrv); + node.Request("/world/default/state_async", req); + + sleep = 0; + // cppcheck-suppress unmatchedSuppression + // cppcheck-suppress knownConditionTrueFalse + while (!received && sleep++ < maxSleep) + { + // Run server + server.Run(true, 1, false); + IGN_SLEEP_MS(100); + } + EXPECT_TRUE(received); } ///////////////////////////////////////////////// diff --git a/test/integration/sdf_include.cc b/test/integration/sdf_include.cc index d573c5f066..29f9c222d8 100644 --- a/test/integration/sdf_include.cc +++ b/test/integration/sdf_include.cc @@ -38,6 +38,6 @@ TEST(SdfInclude, DownloadFromFuel) gazebo::Server server(serverConfig); EXPECT_TRUE(common::exists(path + - "/fuel.ignitionrobotics.org/OpenRobotics/models/ground plane" + + "/fuel.ignitionrobotics.org/openrobotics/models/ground plane" + "/1/model.sdf")); } diff --git a/test/worlds/logical_audio_sensor_plugin.sdf b/test/worlds/logical_audio_sensor_plugin.sdf new file mode 100644 index 0000000000..acd2318c7e --- /dev/null +++ b/test/worlds/logical_audio_sensor_plugin.sdf @@ -0,0 +1,52 @@ + + + + + 0 0 0 0 0 0 + + + + + 1 + linear + sphere + 3.0 + 8.0 + .9 + true + 0 + + + + + + 1 1 0 0 0 0 + + + + + 2 + 0.5 0 0 0 0 0 + .1 + + + + + + 10 1 0 0 0 0 + + + + + 1 + + + + + diff --git a/test/worlds/logical_audio_sensor_plugin_services.sdf b/test/worlds/logical_audio_sensor_plugin_services.sdf new file mode 100644 index 0000000000..5722016b69 --- /dev/null +++ b/test/worlds/logical_audio_sensor_plugin_services.sdf @@ -0,0 +1,46 @@ + + + + + 0 0 0 0 0 0 + + + + + 1 + 0 0 0 0 0 0 + linear + sphere + 1.0 + 2.0 + .5 + true + 0 + + + + + + 10 10 0 0 0 0 + + + + + 2 + 0 0 0 0 0 0 + linear + sphere + 1.0 + 2.0 + .5 + false + 0 + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index 84ef16ad3b..6191d0d020 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -7,27 +7,29 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. **Tutorials** -* \subpage terminology "Terminology": List of terms used across the documentation -* \subpage createsystemplugins "Create System Plugins": Programmatically access simulation using C++ plugins -* \subpage levels "Levels": Load entities on demand in large environments -* \subpage distributedsimulation "Distributed Simulation": Spread simulation across several processes -* \subpage resources "Finding resources": The different ways in which Ignition looks for files +* \subpage terminology "Terminology": List of terms used across the documentation. +* \subpage createsystemplugins "Create System Plugins": Programmatically access simulation using C++ plugins. +* \subpage levels "Levels": Load entities on demand in large environments. +* \subpage distributedsimulation "Distributed Simulation": Spread simulation across several processes. +* \subpage resources "Finding resources": The different ways in which Ignition looks for files. * \subpage log "Logging": Record and play back time series of world state. * \subpage physics "Physics engines": Loading different physics engines. -* \subpage battery "Battery": Keep track of battery charge on robot models +* \subpage battery "Battery": Keep track of battery charge on robot models. * \subpage gui_config "GUI configuration": Customizing your layout. * \subpage debugging "Debugging": Information about debugging Gazebo. -* \subpage pointcloud "Converting a Point Cloud to a 3D Model": Turn point cloud data into 3D models for use in simulations +* \subpage pointcloud "Converting a Point Cloud to a 3D Model": Turn point cloud data into 3D models for use in simulations. * \subpage meshtofuel "Importing a Mesh to Fuel": Build a model directory around a mesh so it can be added to the Ignition Fuel app. -* \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation -* \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation +* \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation. +* \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation. +* \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation. -**Migration from Gazebo-classic** +**Migration from Gazebo classic** -* \subpage migrationplugins "Plugins": Walk through the differences between writing plugins for Gazebo-classic and Ignition Gazebo -* \subpage migrationworldapi "World API": Guide on what World C++ functions to call in Ignition Gazebo when migrating from Gazebo-classic -* \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Ignition Gazebo when migrating from Gazebo-classic -* \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Classic Gazebo to Ignition Gazebo. +* \subpage migrationplugins "Plugins": Walk through the differences between writing plugins for Gazebo classic and Ignition Gazebo +* \subpage migrationworldapi "World API": Guide on what World C++ functions to call in Ignition Gazebo when migrating from Gazebo classic +* \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Ignition Gazebo when migrating from Gazebo classic +* \subpage migrationlinkapi "Link API": Guide on what Link C++ functions to call in Ignition Gazebo when migrating from Gazebo classic +* \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Gazebo classic to Ignition Gazebo. ## License diff --git a/tutorials/files/logical_audio_sensor_plugin.png b/tutorials/files/logical_audio_sensor_plugin.png new file mode 100644 index 0000000000..e29a2ecf11 Binary files /dev/null and b/tutorials/files/logical_audio_sensor_plugin.png differ diff --git a/tutorials/logical_audio_sensor.md b/tutorials/logical_audio_sensor.md new file mode 100644 index 0000000000..0a1e776e47 --- /dev/null +++ b/tutorials/logical_audio_sensor.md @@ -0,0 +1,199 @@ +\page logicalaudiosensor Logical Audio Sensor + +This tutorial will explain how to use the `LogicalAudioSensor` system plugin in Ignition Gazebo. + +The logical audio sensor plugin allows for the usage of logical audio sources and microphones in a simulation environment. +At the end of each simulation step, microphones check if audio was detected by any of the sources in the world. +The logical audio plugin does not play actual audio to a device like speakers, but rather simulates audio being played in environments to see if audio could theoretically be heard at a certain location or not. + +## Setup + +Let's take a look at [logical_audio_sensor_plugin.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/460d2b1cfbf0addf05a1e61c05e1f7a675a83785/examples/worlds/logical_audio_sensor_plugin.sdf), which defines a simulation world with 4 models (in this case, boxes) that have an audio object attached to them. +This world attaches logical audio sources to the `red_box` and `blue_box` models, and attaches logical microphones to the `green_box` and `yellow_box` models. + +Let's take a look at the SDF relevant to the source for `red_box` to understand how to define a logical audio source in SDF: + +```xml + + + 1 + .5 0 0 0 0 0 + linear + sphere + 1.0 + 5.0 + .8 + true + 10 + + +``` + +As we can see, we use a `` tag to define an audio source. +An explanation of all of the tags can be found in the [plugin documentation](https://github.com/ignitionrobotics/ign-gazebo/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130), but there are a few important things to point out: +* `` is used to identify this source when operating on it via services (services will be discussed later). +Since a model can have multiple sources and microphones attached to it, each source attached to a particular model must have a unique ID. +This means that no other sources attached to `red_box` can have an ID of 1, but sources attached to other models can have an ID of 1 (assuming that other models don't already have a source with an ID of 1 attached to it). +* The source's pose is defined relative to the model's pose it's attached to. +In this case, since the `red_box` pose is `(0, 0, 0.5, 0, 0, 0)` relative to the world, and the source pose is `(0.5, 0, 0, 0, 0, 0)` relative to the `red_box` pose, the source's pose with respect to the world is `(0.5, 0, 0.5, 0, 0, 0)`. +* ``, ``, ``, and `` are parameters that define the source's behavior as it travels through space. +More information about how these parameters behave a source's behavior can be found [here](https://docs.unrealengine.com/en-US/Engine/Audio/DistanceModelAttenuation/index.html). + +One other thing to note is that the source attached to the `blue_box` model has a `` of `0`. +This means that this source will play for an infinite amount of simulation time, unless it is stopped manually by the user. + +Let's now take a look at the SDF relevant to the microphone for `green_box` to understand how to define a logical microphone in SDF: + +```xml + + + 1 + 0 .5 0 0 0 0 + .4 + + +``` + +The same rules regarding `` and `` for a logical audio source also apply to a logical microphone. +You can also take a look at the [microphone documentation](https://github.com/ignitionrobotics/ign-gazebo/blob/314477419d2aa946f384204dc99b17d9fcd963b3/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh#L35-L130) for a detailed explanation of the tags embedded in the `` tag. + +## Testing Source and Microphone Behavior + +Let's use the SDF file introduced above (`logical_audio_sensor_plugin.sdf`) to see how sources and microphones interact in an environment. + +Start a simulation that uses this SDF file by running the following command in a terminal: + +``` +ign gazebo logical_audio_sensor_plugin.sdf +``` + +You should see a something like this: + +@image html files/logical_audio_sensor_plugin.png + +### Observing Microphone Detections + +If a logical microphone can detect a source, it will publish a message to a detection topic. +With the simulator still running, open a new terminal and run the following command to see which microphone detection topics are available: + +``` +ign topic -l +``` + +You should see the following detection topics as a part of the output (the `_1` suffix is the ID assigned to the microphones in the SDF): + +``` +/model/green_box/sensor/mic_1/detection +/model/yellow_box/sensor/mic_1/detection +``` + +Let's see if the microphone attached to `yellow_box` can hear anything. +Run the following command: + +``` +ign topic -e -t /model/yellow_box/sensor/mic_1/detection +``` + +You'll notice that this command produces no output. +This means that this microphone cannot detect any of the sources that are currently playing. + +Now, let's see if the microphone attached to `green_box` can hear anything. +Modify the command you just ran to look like this: + +``` +ign topic -e -t /model/green_box/sensor/mic_1/detection +``` + +You'll notice an output that looks like the following: + +``` +header { + stamp { + } + data { + key: "world/logical_audio_sensor/model/red_box/sensor/source_1" + } +} +data: 0.4900980486407216 +``` + +This means that this microphone can detect the source connected to `red_box`, and the volume level detected by the microphone was roughly `0.49` (0.0 being 0% volume, and 1.0 being 100% volume). +Now, go ahead and start simulation by pressing the "play" icon at the bottom-left of the screen, and the detection output will now have a time stamp attached to it: + +``` +header { + stamp { + sec: 8 + nsec: 925000000 + } + data { + key: "world/logical_audio_sensor/model/red_box/sensor/source_1" + } +} +data: 0.4900980486407216 +``` + +Since the source attached to `red_box` was defined in the SDF file using `10`, this means that the last message published by the `green_box` detection topic will have a time stamp of 10 seconds. + +### Starting/Stopping Audio Sources + +Now that we've discussed how to observe microphone detections, let's see how we can modify the state of a logical audio source. +Logical audio sources can be started/stopped manually through Ignition services. +To see which services to call in order to start/stop a service, open a new terminal and run the following command (make sure the simulator is still running): + +``` +ign service -l +``` + +If you look through the list of available services, you should see the following audio source services (the `_1` suffix is the ID assigned to the sources in the SDF): + +``` +/model/blue_box/sensor/source_1/play +/model/blue_box/sensor/source_1/stop +/model/red_box/sensor/source_1/play +/model/red_box/sensor/source_1/stop +``` + +Let's start the source attached to `blue_box`. +This can be done by running the following command, which calls the "play" service for the source attached to `blue_box`: + +``` +ign service -s /model/blue_box/sensor/source_1/play --reqtype ignition.msgs.Empty --reptype ignition.msgs.Boolean --timeout 1000 --req 'unused: false' +``` + +Now, if you look back at the terminal that is displaying the output of `green_box`'s microphone detections, we can see that this microphone is detecting audio from `blue_box`'s source (we see `blue_box` as a part of the `key` field): + +``` +header { + stamp { + sec: 255 + nsec: 673000000 + } + data { + key: "world/logical_audio_sensor/model/blue_box/sensor/source_1" + } +} +data: 0.6 +``` + +You can also echo `yellow_box`'s microphone detection topic, and you should see that this microphone can detect audio from `blue_box`'s source as well. + +Since the source attached to `blue_box` was configured with `0` in SDF, it won't stop playing unless we call the stop service on it. +Before we call the stop service on this source, move `blue_box` around with the transform control tool. +You'll see that once `blue_box`'s source volume falls below a microphone's detection threshold (defined in the SDF via ``), messages will stop being published to the microphone's detection topic. + +Let's go ahead and call the stop service to make sure that this source will stop playing. +Move `blue_box` back towards its original position, until you see detection messages being published by the microphones attached to `green_box` and/or `yellow_box`. +Now, go ahead and stop `blue_box`'s source by running the following command: + +``` +ign service -s /model/blue_box/sensor/source_1/stop --reqtype ignition.msgs.Empty --reptype ignition.msgs.Boolean --timeout 1000 --req 'unused: false' +``` + +Now, if you look at the output for either microphone topic, you'll notice that no new messages are being published, which makes sense since no audio sources are currently playing. +You can also try to call the play/stop services on the source attached to the `red_box`. +Since the `red_box` has a play duration of 10 seconds, the source will automatically stop playing 10 seconds (of sim time) after starting it, unless you stop it earlier by calling the stop service. +One other important thing to note is that calling the play service on a source that is already playing does nothing. +For example, if the source attached to `red_box` is already playing, and you call the play service on it, then it will not play for another 10 seconds. +The source will still stop at the original 10 second duration mark. +Similarly, calling the stop service on a source that is already stopped also does nothing.