diff --git a/examples/standalone/scene_provider/README.md b/examples/standalone/scene_provider/README.md index 9c6d06291..6a823fab4 100644 --- a/examples/standalone/scene_provider/README.md +++ b/examples/standalone/scene_provider/README.md @@ -9,7 +9,7 @@ plugin to update the scene using Gazebo Transport. ## Build -``` +```bash cd examples/standalone/scene_provider mkdir build cd build @@ -21,14 +21,14 @@ make In one terminal, start the scene provider: -``` +```bash cd examples/standalone/scene_provider/build ./scene_provider ``` On another terminal, start the example config: -``` +```bash gz gui -c examples/config/scene3d.config ``` @@ -42,36 +42,36 @@ Some commands to test camera tracking with this demo: Move to box: -``` +```bash gz service -s /gui/move_to --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"' ``` Echo camera pose: -``` +```bash gz topic -e -t /gui/camera/pose ``` Follow box: -``` +```bash gz service -s /gui/follow --reqtype gz.msgs.StringMsg --reptype gz.msgs.Boolean --timeout 2000 --req 'data: "box_model"' ``` Update follow offset: -``` +```bash gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 5, y: 5, z: 5' ``` -Set next follow pgain: +Set next follow p_gain: -``` -gz service -s /gui/follow/pgain --reqtype gz.msgs.Double --reptype gz.msgs.Boolean --timeout 2000 --req 'data: 1.0' +```bash +gz service -s /gui/follow/p_gain --reqtype gz.msgs.Double --reptype gz.msgs.Boolean --timeout 2000 --req 'data: 1.0' ``` -Update follow offset with new pgain: +Update follow offset with new p_gain: -``` +```bash gz service -s /gui/follow/offset --reqtype gz.msgs.Vector3d --reptype gz.msgs.Boolean --timeout 2000 --req 'x: 1, y: 1, z: 5' ``` \ No newline at end of file diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index ff282d6b9..e05a6f39b 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -113,6 +113,7 @@ endfunction() # Plugins add_subdirectory(camera_tracking) +add_subdirectory(follow_config) add_subdirectory(grid_config) add_subdirectory(image_display) add_subdirectory(interactive_view_control) diff --git a/src/plugins/camera_tracking/CameraTracking.cc b/src/plugins/camera_tracking/CameraTracking.cc index 6df3fe677..184cd536a 100644 --- a/src/plugins/camera_tracking/CameraTracking.cc +++ b/src/plugins/camera_tracking/CameraTracking.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 Rudis Laboratories LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -226,7 +227,7 @@ void CameraTrackingPrivate::Initialize() << this->followOffsetService << "]" << std::endl; // follow pgain - this->followPGainService = "/gui/follow/pgain"; + this->followPGainService = "/gui/follow/p_gain"; this->node.Advertise(this->followPGainService, &CameraTrackingPrivate::OnFollowPGain, this); gzmsg << "Follow P gain service on [" @@ -289,8 +290,11 @@ bool CameraTrackingPrivate::OnFollowPGain(const msgs::Double &_msg, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); - this->followPGain = msgs::Convert(_msg); - + if (!this->followTarget.empty()) + { + this->newFollowOffset = true; + this->followPGain = msgs::Convert(_msg); + } _res.set_data(true); return true; } diff --git a/src/plugins/camera_tracking/CameraTracking.hh b/src/plugins/camera_tracking/CameraTracking.hh index d7f9752a6..894e4f003 100644 --- a/src/plugins/camera_tracking/CameraTracking.hh +++ b/src/plugins/camera_tracking/CameraTracking.hh @@ -39,7 +39,7 @@ namespace plugins /// * `/gui/move_to/pose`: Move the user camera to a given pose. /// * `/gui/follow`: Set the user camera to follow a given target, /// identified by name. - /// * `/gui/follow/pgain`: Set the pgain for following. + /// * `/gui/follow/p_gain`: Set the pgain for following. /// * `/gui/follow/offset`: Set the offset for following. /// /// Topics: diff --git a/src/plugins/camera_tracking/CameraTracking.qml b/src/plugins/camera_tracking/CameraTracking.qml index 07d5c4517..dd1749641 100644 --- a/src/plugins/camera_tracking/CameraTracking.qml +++ b/src/plugins/camera_tracking/CameraTracking.qml @@ -29,7 +29,7 @@ ColumnLayout { '
  • /gui/move_to
  • ' + '
  • /gui/move_to/pose
  • ' + '
  • /gui/follow
  • ' + - '
  • /gui/follow/pgain
  • ' + + '
  • /gui/follow/p_gain
  • ' + '
  • /gui/follow/offset

  • Topics provided:
    ' diff --git a/src/plugins/follow_config/CMakeLists.txt b/src/plugins/follow_config/CMakeLists.txt new file mode 100644 index 000000000..6710b1174 --- /dev/null +++ b/src/plugins/follow_config/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_gui_add_plugin(FollowConfig + SOURCES + FollowConfig.cc + QT_HEADERS + FollowConfig.hh +) diff --git a/src/plugins/follow_config/FollowConfig.cc b/src/plugins/follow_config/FollowConfig.cc new file mode 100644 index 000000000..5f88bcd2b --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.cc @@ -0,0 +1,265 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * 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 "gz/gui/Application.hh" +#include "gz/gui/Conversions.hh" +#include "gz/gui/GuiEvents.hh" +#include "gz/gui/MainWindow.hh" + +#include + +#include "FollowConfig.hh" + +/// \brief Private data class for FollowConfig +class gz::gui::plugins::FollowConfigPrivate +{ + /// \brief Service request topic for follow name + public: std::string followTargetNameService; + + /// \brief Service request topic for follow offset + public: std::string followOffsetService; + + /// \brief Service request topic for follow p_gain + public: std::string followPGainService; + + /// \brief Offset of camera from target being followed + public: math::Vector3d followOffset{math::Vector3d(-5.0, 0.0, 3.0)}; + + /// \brief Follow P gain + public: double followPGain{0.01}; + + /// \brief Follow target name from sdf + public: std::string followTargetName; + + public: transport::Node node; + + /// \brief Process updated follow name from SDF + public: void UpdateFollowTargetName(); + + /// \brief Process updated follow offset + public: void UpdateFollowOffset(); + + /// \brief Process updated P Gain + public: void UpdateFollowPGain(); + + /// \brief flag for updating target name + public: bool newFollowUpdateTargetName = false; + + /// \brief flag for updating P gain + public: bool newFollowUpdatePGain = false; + + /// \brief flag for updating offset + public: bool newFollowUpdateOffset = false; + +}; + +using namespace gz; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +FollowConfig::FollowConfig() + : gz::gui::Plugin(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +FollowConfig::~FollowConfig() = default; + +///////////////////////////////////////////////// +void FollowConfig::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Follow Config"; + + // Follow target name service + this->dataPtr->followTargetNameService = "/gui/follow"; + gzmsg << "FollowConfig: Follow target name service on [" + << this->dataPtr->followTargetNameService << "]" << std::endl; + + // Follow target offset service + this->dataPtr->followOffsetService = "/gui/follow/offset"; + gzmsg << "FollowConfig: Follow offset service on [" + << this->dataPtr->followOffsetService << "]" << std::endl; + + // Follow target pgain service + this->dataPtr->followPGainService = "/gui/follow/p_gain"; + gzmsg << "FollowConfig: Follow P gain service on [" + << this->dataPtr->followPGainService << "]" << std::endl; + + + // Read configuration + if (_pluginElem) + { + if (auto nameElem = _pluginElem->FirstChildElement("follow_target")) + { + this->dataPtr->followTargetName = nameElem->GetText(); + gzmsg << "FollowConfig: Loaded follow_target from sdf [" + << this->dataPtr->followTargetName << "]" << std::endl; + this->dataPtr->newFollowUpdateTargetName = true; + } + if (auto offsetElem = _pluginElem->FirstChildElement("follow_offset")) + { + std::stringstream offsetStr; + offsetStr << std::string(offsetElem->GetText()); + offsetStr >> this->dataPtr->followOffset; + gzmsg << "FollowConfig: Loaded follow_offset from sdf [" + << this->dataPtr->followOffset << "]" << std::endl; + this->dataPtr->newFollowUpdateOffset = true; + } + if (auto pGainElem = _pluginElem->FirstChildElement("follow_pgain")) + { + this->dataPtr->followPGain = std::stod(std::string(pGainElem->GetText())); + gzmsg << "FollowConfig: Loaded follow_pgain from sdf [" + << this->dataPtr->followPGain << "]" << std::endl; + this->dataPtr->newFollowUpdatePGain = true; + } + } + + gui::App()->findChild< + MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool FollowConfig::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == events::Render::kType) + { + if (this->dataPtr->newFollowUpdateTargetName) + { + this->dataPtr->UpdateFollowTargetName(); + } + if (this->dataPtr->newFollowUpdatePGain) + { + this->dataPtr->UpdateFollowPGain(); + } + if (this->dataPtr->newFollowUpdateOffset) + { + this->dataPtr->UpdateFollowOffset(); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void FollowConfig::SetFollowOffset(double _x, + double _y, double _z) +{ + if (!this->dataPtr->newFollowUpdateOffset) + { + this->dataPtr->followOffset = math::Vector3d( + _x, _y, _z); + gzmsg << "FollowConfig: SetFollowOffset(" + << this->dataPtr->followOffset << ")" << std::endl; + this->dataPtr->newFollowUpdateOffset = true; + } +} + +///////////////////////////////////////////////// +void FollowConfig::SetFollowPGain(double _p) +{ + if (!this->dataPtr->newFollowUpdatePGain) + { + this->dataPtr->followPGain = _p; + gzmsg << "FollowConfig: SetFollowPGain(" + << this->dataPtr->followPGain << ")" << std::endl; + this->dataPtr->newFollowUpdatePGain = true; + } +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowTargetName() +{ + // Offset + std::function cbName = + [&](const msgs::Boolean &/*_rep*/, const bool _resultName) + { + if (!_resultName) + { + gzerr << "FollowConfig: Error sending follow target name." << std::endl; + } else { + gzmsg << "FollowConfig: Request Target Name: " + << this->followTargetName << " sent" << std::endl; + } + }; + + msgs::StringMsg reqName; + reqName.set_data(this->followTargetName); + node.Request(this->followTargetNameService, reqName, cbName); + this->newFollowUpdateTargetName = false; +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowOffset() +{ + // Offset + std::function cbOffset = + [&](const msgs::Boolean &/*_rep*/, const bool _resultOffset) + { + if (!_resultOffset) + { + gzerr << "FollowConfig: Error sending follow offset." << std::endl; + } else { + gzmsg << "FollowConfig: Request Offset: " + << this->followOffset << " sent" << std::endl; + } + }; + + msgs::Vector3d reqOffset; + reqOffset.set_x(this->followOffset.X()); + reqOffset.set_y(this->followOffset.Y()); + reqOffset.set_z(this->followOffset.Z()); + node.Request(this->followOffsetService, reqOffset, cbOffset); + this->newFollowUpdateOffset = false; +} + +///////////////////////////////////////////////// +void FollowConfigPrivate::UpdateFollowPGain() +{ + // PGain + std::function cbPGain = + [&](const msgs::Boolean &/*_rep*/, const bool _resultPGain) + { + if (!_resultPGain) + { + gzerr << "FollowConfig: Error sending follow pgain." << std::endl; + } else { + gzmsg << "FollowConfig: Request PGain: " + << this->followPGain << " sent" << std::endl; + } + }; + + msgs::Double reqPGain; + reqPGain.set_data(this->followPGain); + node.Request(this->followPGainService, reqPGain, cbPGain); + this->newFollowUpdatePGain = false; +} + +// Register this plugin +GZ_ADD_PLUGIN(FollowConfig, + gui::Plugin) diff --git a/src/plugins/follow_config/FollowConfig.hh b/src/plugins/follow_config/FollowConfig.hh new file mode 100644 index 000000000..e4f2a02c4 --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_ +#define GZ_GUI_PLUGINS_FOLLOWCONFIG_HH_ + +#include + +#include "gz/gui/Plugin.hh" + +namespace gz +{ +namespace gui +{ +namespace plugins +{ + class FollowConfigPrivate; + + class FollowConfig : public Plugin + { + Q_OBJECT + + /// \brief Constructor + public: FollowConfig(); + + /// \brief Destructor + public: virtual ~FollowConfig(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + /// \brief Set the follow offset, requested from the GUI. + /// \param[in] _x The follow offset distance in x + /// \param[in] _y The follow offset distance in y + /// \param[in] _z The follow offset distance in z + public slots: void SetFollowOffset(double _x, + double _y, double _z); + + /// \brief Set the follow pgain, requested from the GUI. + /// \param[in] _p The follow offset distance in x + public slots: void SetFollowPGain(double _p); + + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/plugins/follow_config/FollowConfig.qml b/src/plugins/follow_config/FollowConfig.qml new file mode 100644 index 000000000..f9eff9b9c --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.qml @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * 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.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Layouts 1.3 +import gz.gui 1.0 + +ColumnLayout { + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + Layout.margins: 2 + anchors.fill: parent + focus: true + + // X camera follow distance + property double xFollowOffset: -5.0 + // Y camera follow distance + property double yFollowOffset: 0.0 + // Z camera follow distance + property double zFollowOffset: 3.0 + // P Gain camera follow distance + property double pGainFollow: 0.01 + + GridLayout { + Layout.fillWidth: true + Layout.margins: 2 + columns: 2 + + // X follow offset distance + Label { + id: xFollowOffsetLabel + text: "Camera follow X (m)" + color: "dimgrey" + } + GzSpinBox { + id: xFollowOffsetField + Layout.fillWidth: true + value: xFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + xFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // Y follow offset distance + Label { + id: yFollowOffsetLabel + text: "Camera follow Y (m)" + color: "dimgrey" + } + GzSpinBox { + id: yFollowOffsetField + Layout.fillWidth: true + value: yFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + yFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // Z follow offset distance + Label { + id: zFollowOffsetLabel + text: "Camera follow Z (m)" + color: "dimgrey" + } + GzSpinBox { + id: zFollowOffsetField + Layout.fillWidth: true + value: zFollowOffset + maximumValue: 10.0 + minimumValue: -10.0 + decimals: 2 + stepSize: 0.5 + onEditingFinished:{ + zFollowOffset = value + FollowConfig.SetFollowOffset(xFollowOffset, yFollowOffset, zFollowOffset) + } + } + // P Gain follow + Label { + id: pGainFollowLabel + text: "Camera follow P Gain" + color: "dimgrey" + } + GzSpinBox { + id: pGainFollowField + Layout.fillWidth: true + value: pGainFollow + maximumValue: 1.0 + minimumValue: 0.001 + decimals: 3 + stepSize: 0.01 + onEditingFinished:{ + pGainFollow = value + FollowConfig.SetFollowPGain(pGainFollow) + } + } + } +} diff --git a/src/plugins/follow_config/FollowConfig.qrc b/src/plugins/follow_config/FollowConfig.qrc new file mode 100644 index 000000000..61099f8f1 --- /dev/null +++ b/src/plugins/follow_config/FollowConfig.qrc @@ -0,0 +1,5 @@ + + + FollowConfig.qml + + diff --git a/test/integration/camera_tracking.cc b/test/integration/camera_tracking.cc index 895f2c563..6321ef41b 100644 --- a/test/integration/camera_tracking.cc +++ b/test/integration/camera_tracking.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -188,7 +189,7 @@ TEST(MinimalSceneTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) msgs::Double reqPGain; reqPGain.set_data(1.0); - executed = node.Request("/gui/follow/pgain", reqPGain, timeout, rep, + executed = node.Request("/gui/follow/p_gain", reqPGain, timeout, rep, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); diff --git a/test/integration/follow_config.cc b/test/integration/follow_config.cc new file mode 100644 index 000000000..ed30df78c --- /dev/null +++ b/test/integration/follow_config.cc @@ -0,0 +1,167 @@ +/* + * Copyright (C) 2023 Rudis Laboratories LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "test_config.hh" // NOLINT(build/include) +#include "gz/gui/Application.hh" +#include "gz/gui/GuiEvents.hh" +#include "gz/gui/Plugin.hh" +#include "gz/gui/MainWindow.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./follow_config")), +}; + +using namespace gz; +using namespace gui; +using namespace std::chrono_literals; + +///////////////////////////////////////////////// +TEST(MinimalSceneTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Config)) +{ + common::Console::SetVerbosity(4); + + Application app(g_argc, g_argv); + app.AddPluginPath(gz::common::joinPaths( + std::string(PROJECT_BINARY_PATH), "lib")); + + // Load plugins + const char *pluginStr = + "" + "ogre" + "banana" + "1.0 0 0" + "0 1 0" + "0 0 0 0 0 0" + ""; + + tinyxml2::XMLDocument pluginDoc; + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("MinimalScene", + pluginDoc.FirstChildElement("plugin"))); + + pluginStr = + "" + ""; + + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("CameraTracking", + pluginDoc.FirstChildElement("plugin"))); + + pluginStr = + "" + "track_me" + "0.0 0.0 0.0" + "1.0" + ""; + + pluginDoc.Parse(pluginStr); + EXPECT_TRUE(app.LoadPlugin("FollowConfig", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto win = app.findChild(); + ASSERT_NE(nullptr, win); + + // Get plugin + auto plugins = win->findChildren(); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Get camera pose + msgs::Pose poseMsg; + auto poseCb = std::function( + [&](const auto &_msg) + { + poseMsg = _msg; + }); + + transport::Node node; + node.Subscribe("/gui/camera/pose", poseCb); + + int sleep = 0; + int maxSleep = 60; + while (!poseMsg.has_position() && sleep++ < maxSleep) + { + std::this_thread::sleep_for(100ms); + QCoreApplication::processEvents(); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_TRUE(poseMsg.has_position()); + EXPECT_TRUE(poseMsg.has_orientation()); + + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + auto scene = engine->SceneByName("banana"); + ASSERT_NE(nullptr, scene); + + auto trackedVis = scene->CreateVisual("track_me"); + ASSERT_NE(nullptr, trackedVis); + trackedVis->SetWorldPose({0, 0, 0, 0, 0, 0}); + + auto root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + auto camera = std::dynamic_pointer_cast( + root->ChildByIndex(0)); + ASSERT_NE(nullptr, camera); + + msgs::StringMsg req; + msgs::Boolean rep; + req.set_data("track_me"); + + bool result; + unsigned int timeout = 2000; + bool executed = node.Request("/gui/follow", req, timeout, rep, result); + EXPECT_TRUE(executed); + EXPECT_TRUE(result); + EXPECT_TRUE(rep.data()); + + sleep = 0; + while (abs(poseMsg.mutable_position()->x()) < .01 && sleep++ < maxSleep) + { + std::this_thread::sleep_for(100ms); + QCoreApplication::processEvents(); + } + EXPECT_LT(sleep, maxSleep); + + EXPECT_NEAR(0.0, abs(poseMsg.mutable_position()->x()), 1.0); + EXPECT_NEAR(0.0, abs(poseMsg.mutable_position()->y()), 1.0); + EXPECT_NEAR(0.0, abs(poseMsg.mutable_position()->z()), 1.0); +}