diff --git a/examples/config/scene3d.config b/examples/config/scene3d.config index 1f6caa286..afb673afc 100644 --- a/examples/config/scene3d.config +++ b/examples/config/scene3d.config @@ -32,6 +32,20 @@ /example/delete /example/scene + + + + + + + false + 5 + 5 + floating + false + + /example/stats + diff --git a/examples/standalone/marker/CMakeLists.txt b/examples/standalone/marker/CMakeLists.txt new file mode 100644 index 000000000..c9bd8262f --- /dev/null +++ b/examples/standalone/marker/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) + set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) + + find_package(ignition-common4 REQUIRED) + set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) + + find_package(ignition-msgs8 REQUIRED) + set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) + + add_executable(marker marker.cc) + target_link_libraries(marker + ignition-transport${IGN_TRANSPORT_VER}::core + ignition-msgs${IGN_MSGS_VER} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ) +endif() diff --git a/examples/standalone/marker/README.md b/examples/standalone/marker/README.md new file mode 100644 index 000000000..a0c5bd565 --- /dev/null +++ b/examples/standalone/marker/README.md @@ -0,0 +1,22 @@ +# Ignition Visualization Marker Example + +This example demonstrates how to create, modify, and delete visualization +markers in Ignition GUI. + +## Build Instructions + +From this directory: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +Launch ign gazebo unpaused then from the build directory above: + + ./marker + +The terminal will output messages indicating visualization changes that +will occur in Ignition GUI's render window. diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc new file mode 100644 index 000000000..ec57227e8 --- /dev/null +++ b/examples/standalone/marker/marker.cc @@ -0,0 +1,315 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include + +#include + +///////////////////////////////////////////////// +int main(int _argc, char **_argv) +{ + ignition::transport::Node node; + + // Create the marker message + ignition::msgs::Marker markerMsg; + ignition::msgs::Material matMsg; + markerMsg.set_ns("default"); + markerMsg.set_id(0); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::SPHERE); + markerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Set color to Blue + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(1); + markerMsg.mutable_material()->mutable_ambient()->set_a(1); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(1); + markerMsg.mutable_material()->mutable_diffuse()->set_a(1); + markerMsg.mutable_lifetime()->set_sec(2); + markerMsg.mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); + + // The rest of this function adds different shapes and/or modifies shapes. + // Read the terminal statements to figure out what each node.Request + // call accomplishes. + std::cout << "Spawning a blue sphere with lifetime 2s\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(2, 2, 0, 0, 0, 0)); + node.Request("/marker", markerMsg); + std::cout << "Sleeping for 2 seconds\n"; + std::this_thread::sleep_for(std::chrono::seconds(2)); + + std::cout << "Spawning a black sphere at the origin with no lifetime\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(1); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d::Zero); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + markerMsg.mutable_lifetime()->set_sec(0); + node.Request("/marker", markerMsg); + + std::cout << "Moving the black sphere to x=0, y=1, z=1\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(0, 1, 1, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Shrinking the black sphere\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(0.2, 0.2, 0.2)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the black sphere to red\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(1); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(1); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + std::this_thread::sleep_for(std::chrono::seconds(4)); + node.Request("/marker", markerMsg); + + std::cout << "Adding a green ellipsoid\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(1); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(1); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(2); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::SPHERE); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(0.5, 1.0, 1.5)); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(2, 0, .5, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the green ellipsoid to a cylinder\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_type(ignition::msgs::Marker::CYLINDER); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(0.5, 0.5, 1.5)); + node.Request("/marker", markerMsg); + + std::cout << "Connecting the sphere and cylinder with a line\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(3); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0.0, 1.0, 1.0)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(2, 0, 0.5)); + node.Request("/marker", markerMsg); + + std::cout << "Adding a square around the origin\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(4); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::LINE_STRIP); + ignition::msgs::Set(markerMsg.mutable_point(0), + ignition::math::Vector3d(0.5, 0.5, 0.05)); + ignition::msgs::Set(markerMsg.mutable_point(1), + ignition::math::Vector3d(0.5, -0.5, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(-0.5, -0.5, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(-0.5, 0.5, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0.5, 0.5, 0.05)); + node.Request("/marker", markerMsg); + + std::cout << "Adding 100 points inside the square\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(5); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::POINTS); + markerMsg.clear_point(); + for (int i = 0; i < 100; ++i) + { + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d( + ignition::math::Rand::DblUniform(-0.49, 0.49), + ignition::math::Rand::DblUniform(-0.49, 0.49), + 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding a semi-circular triangle fan\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(6); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_FAN); + markerMsg.clear_point(); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(0, 1.5, 0, 0, 0, 0)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0, 0, 0.05)); + double radius = 2; + for (double t = 0; t <= M_PI; t+= 0.01) + { + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding two triangles using a triangle list\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(7); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_LIST); + markerMsg.clear_point(); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(0, -1.5, 0, 0, 0, 0)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0, 0, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 0, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 1, 0.05)); + + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 1, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(2, 1, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(2, 2, 0.05)); + + node.Request("/marker", markerMsg); + + std::cout << "Adding a rectangular triangle strip\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_id(8); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_STRIP); + markerMsg.clear_point(); + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(-2, -2, 0, 0, 0, 0)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0, 0, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 0, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0, 1, 0.05)); + + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 1, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(0, 2, 0.05)); + ignition::msgs::Set(markerMsg.add_point(), + ignition::math::Vector3d(1, 2, 0.05)); + + node.Request("/marker", markerMsg); + std::cout << "Adding multiple markers via /marker_array\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + + ignition::msgs::Marker_V markerMsgs; + ignition::msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + + // Create first blue sphere marker + auto markerMsg1 = markerMsgs.add_marker(); + markerMsg1->set_ns("default"); + markerMsg1->set_id(0); + markerMsg1->set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg1->set_type(ignition::msgs::Marker::SPHERE); + markerMsg1->set_visibility(ignition::msgs::Marker::GUI); + + // Set color to Blue + markerMsg1->mutable_material()->mutable_ambient()->set_r(0); + markerMsg1->mutable_material()->mutable_ambient()->set_g(0); + markerMsg1->mutable_material()->mutable_ambient()->set_b(1); + markerMsg1->mutable_material()->mutable_ambient()->set_a(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_b(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_a(1); + ignition::msgs::Set(markerMsg1->mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); + ignition::msgs::Set(markerMsg1->mutable_pose(), + ignition::math::Pose3d(3, 3, 0, 0, 0, 0)); + + // Create second red box marker + auto markerMsg2 = markerMsgs.add_marker(); + markerMsg2->set_ns("default"); + markerMsg2->set_id(0); + markerMsg2->set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg2->set_type(ignition::msgs::Marker::BOX); + markerMsg2->set_visibility(ignition::msgs::Marker::GUI); + + // Set color to Red + markerMsg2->mutable_material()->mutable_ambient()->set_r(1); + markerMsg2->mutable_material()->mutable_ambient()->set_g(0); + markerMsg2->mutable_material()->mutable_ambient()->set_b(0); + markerMsg2->mutable_material()->mutable_ambient()->set_a(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_r(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg2->mutable_lifetime()->set_sec(2); + markerMsg2->mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(markerMsg2->mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); + ignition::msgs::Set(markerMsg2->mutable_pose(), + ignition::math::Pose3d(3, 3, 2, 0, 0, 0)); + + // Create green capsule marker + auto markerMsg3 = markerMsgs.add_marker(); + markerMsg3->set_ns("default"); + markerMsg3->set_id(0); + markerMsg3->set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg3->set_type(ignition::msgs::Marker::CAPSULE); + markerMsg3->set_visibility(ignition::msgs::Marker::GUI); + + // Set color to Green + markerMsg3->mutable_material()->mutable_ambient()->set_r(0); + markerMsg3->mutable_material()->mutable_ambient()->set_g(1); + markerMsg3->mutable_material()->mutable_ambient()->set_b(0); + markerMsg3->mutable_material()->mutable_ambient()->set_a(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_g(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg3->mutable_lifetime()->set_sec(2); + markerMsg3->mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(markerMsg3->mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); + ignition::msgs::Set(markerMsg3->mutable_pose(), + ignition::math::Pose3d(3, 3, 4, 0, 0, 0)); + + // Publish the three created markers above simultaneously + node.Request("/marker_array", markerMsgs, timeout, res, result); + + std::cout << "Deleting all the markers\n"; + std::this_thread::sleep_for(std::chrono::seconds(4)); + markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + node.Request("/marker", markerMsg); +} diff --git a/examples/standalone/scene_provider/scene_provider.cc b/examples/standalone/scene_provider/scene_provider.cc index 5f32eab8e..2bb4a9ab1 100644 --- a/examples/standalone/scene_provider/scene_provider.cc +++ b/examples/standalone/scene_provider/scene_provider.cc @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include using namespace std::chrono_literals; @@ -75,6 +77,10 @@ int main(int argc, char **argv) // Scene service node.Advertise("/example/scene", sceneService); + // Periodic pose updated + auto statsPub = + node.Advertise("/example/stats"); + // Periodic pose updated auto posePub = node.Advertise("/example/pose"); @@ -90,6 +96,10 @@ int main(int argc, char **argv) double y{0.0}; double z{0.0}; + std::chrono::steady_clock::duration timePoint = + std::chrono::steady_clock::duration::zero(); + ignition::msgs::WorldStatistics msgWorldStatistics; + while (true) { std::this_thread::sleep_for(100ms); @@ -102,6 +112,16 @@ int main(int argc, char **argv) positionMsg->set_y(y); positionMsg->set_z(z); posePub.Publish(poseVMsg); + + timePoint += 100ms; + msgWorldStatistics.set_real_time_factor(1); + + auto s = std::chrono::duration_cast(timePoint); + auto ns = std::chrono::duration_cast(timePoint-s); + + msgWorldStatistics.mutable_sim_time()->set_sec(s.count()); + msgWorldStatistics.mutable_sim_time()->set_nsec(ns.count()); + statsPub.Publish(msgWorldStatistics); } ignition::transport::waitForShutdown(); diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 68464ff2f..a1d814e29 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -121,6 +121,7 @@ add_subdirectory(image_display) add_subdirectory(key_publisher) add_subdirectory(plotting) add_subdirectory(publisher) +add_subdirectory(marker_manager) add_subdirectory(minimal_scene) add_subdirectory(scene3d) add_subdirectory(screenshot) diff --git a/src/plugins/marker_manager/CMakeLists.txt b/src/plugins/marker_manager/CMakeLists.txt new file mode 100644 index 000000000..42713a762 --- /dev/null +++ b/src/plugins/marker_manager/CMakeLists.txt @@ -0,0 +1,8 @@ +ign_gui_add_plugin(MarkerManager + SOURCES + MarkerManager.cc + QT_HEADERS + MarkerManager.hh + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) diff --git a/src/plugins/marker_manager/MarkerManager.cc b/src/plugins/marker_manager/MarkerManager.cc new file mode 100644 index 000000000..a78531c2a --- /dev/null +++ b/src/plugins/marker_manager/MarkerManager.cc @@ -0,0 +1,752 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include + +#include "ignition/rendering/Marker.hh" +#include +#include + +#include + +#include "ignition/gui/Application.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/Helpers.hh" +#include "ignition/gui/MainWindow.hh" + +#include "MarkerManager.hh" + +/// \brief Private data class for MarkerManager +class ignition::gui::plugins::MarkerManagerPrivate +{ + /// \brief Update markers based on msgs received + public: void OnRender(); + + /// \brief Initialize services and subcriptions + public: void Initialize(); + + /// \brief Processes a marker message. + /// \param[in] _msg The message data. + /// \return True if the marker was processed successfully. + public: bool ProcessMarkerMsg(const ignition::msgs::Marker &_msg); + + /// \brief Services callback that returns a list of markers. + /// \param[out] _rep Service reply + /// \return True on success. + public: bool OnList(ignition::msgs::Marker_V &_rep); + + /// \brief Callback that receives marker messages. + /// \param[in] _req The marker message. + public: void OnMarkerMsg(const ignition::msgs::Marker &_req); + + /// \brief Callback that receives multiple marker messages. + /// \param[in] _req The vector of marker messages + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnMarkerMsgArray(const ignition::msgs::Marker_V &_req, + ignition::msgs::Boolean &_res); + + /// \brief Subscriber callback when new world statistics are received + public: void OnWorldStatsMsg(const ignition::msgs::WorldStatistics &_msg); + + /// \brief Sets Visual from marker message. + /// \param[in] _msg The message data. + /// \param[out] _visualPtr The visual pointer to set. + public: void SetVisual(const ignition::msgs::Marker &_msg, + const rendering::VisualPtr &_visualPtr); + + /// \brief Sets Marker from marker message. + /// \param[in] _msg The message data. + /// \param[out] _markerPtr The message pointer to set. + public: void SetMarker(const ignition::msgs::Marker &_msg, + const rendering::MarkerPtr &_markerPtr); + + /// \brief Converts an ignition msg material to ignition rendering + // material. + // \param[in] _msg The message data. + // \return Converted rendering material, if any. + public: rendering::MaterialPtr MsgToMaterial( + const ignition::msgs::Marker &_msg); + + /// \brief Converts an ignition msg render type to ignition rendering + /// \param[in] _msg The message data + /// \return Converted rendering type, if any. + public: ignition::rendering::MarkerType MsgToType( + const ignition::msgs::Marker &_msg); + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief Mutex to protect message list. + public: std::mutex mutex; + + /// \brief List of marker message to process. + public: std::list markerMsgs; + + /// \brief Map of visuals + public: std::map> visuals; + + /// \brief Ignition node + public: ignition::transport::Node node; + + /// \brief Topic name for the marker service + public: std::string topicName = "/marker"; + + /// \brief Sim time according to world stats message + public: std::chrono::steady_clock::duration simTime; + + /// \brief Previous sim time received + public: std::chrono::steady_clock::duration lastSimTime; + + /// \brief The last marker message received + public: ignition::msgs::Marker msg; +}; + +using namespace ignition; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +void MarkerManagerPrivate::Initialize() +{ + if (!this->scene) + { + ignerr << "Scene pointer is invalid" << std::endl; + return; + } + + if (this->topicName.empty()) + { + ignerr << "Unable to advertise marker service. Topic name empty." + << std::endl; + return; + } + + // Advertise the list service + if (!this->node.Advertise(this->topicName + "/list", + &MarkerManagerPrivate::OnList, this)) + { + ignerr << "Unable to advertise to the " << this->topicName + << "/list service.\n"; + } + + igndbg << "Advertise " << this->topicName << "/list service.\n"; + + // Advertise to the marker service + if (!this->node.Advertise(this->topicName, + &MarkerManagerPrivate::OnMarkerMsg, this)) + { + ignerr << "Unable to advertise to the " << this->topicName + << " service.\n"; + } + + igndbg << "Advertise " << this->topicName << "/list.\n"; + + // Advertise to the marker_array service + if (!this->node.Advertise(this->topicName + "_array", + &MarkerManagerPrivate::OnMarkerMsgArray, this)) + { + ignerr << "Unable to advertise to the " << this->topicName + << "_array service.\n"; + } + + igndbg << "Advertise " << this->topicName << "_array.\n"; +} + +///////////////////////////////////////////////// +void MarkerManagerPrivate::OnRender() +{ + if (!this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + this->Initialize(); + } + + std::lock_guard lock(this->mutex); + // Process the marker messages. + for (auto markerIter = this->markerMsgs.begin(); + markerIter != this->markerMsgs.end();) + { + this->ProcessMarkerMsg(*markerIter); + this->markerMsgs.erase(markerIter++); + } + + // Erase any markers that have a lifetime. + for (auto mit = this->visuals.begin(); + mit != this->visuals.end();) + { + for (auto it = mit->second.cbegin(); + it != mit->second.cend(); ++it) + { + if (it->second->GeometryCount() == 0u) + continue; + + ignition::rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast + (it->second->GeometryByIndex(0u)); + if (markerPtr != nullptr) + { + if (markerPtr->Lifetime().count() != 0 && + (markerPtr->Lifetime() <= this->simTime || + this->simTime < this->lastSimTime)) + { + this->scene->DestroyVisual(it->second); + it = mit->second.erase(it); + break; + } + } + } + + // Erase a namespace if it's empty + if (mit->second.empty()) + mit = this->visuals.erase(mit); + else + ++mit; + } + this->lastSimTime = this->simTime; +} + +///////////////////////////////////////////////// +bool MarkerManagerPrivate::OnList(ignition::msgs::Marker_V &_rep) +{ + std::lock_guard lock(this->mutex); + _rep.clear_marker(); + + // Create the list of visuals + for (auto mIter : this->visuals) + { + for (auto iter : mIter.second) + { + ignition::msgs::Marker *markerMsg = _rep.add_marker(); + markerMsg->set_ns(mIter.first); + markerMsg->set_id(iter.first); + } + } + + return true; +} + +///////////////////////////////////////////////// +void MarkerManagerPrivate::OnMarkerMsg(const ignition::msgs::Marker &_req) +{ + std::lock_guard lock(this->mutex); + this->markerMsgs.push_back(_req); +} + +///////////////////////////////////////////////// +bool MarkerManagerPrivate::OnMarkerMsgArray( + const ignition::msgs::Marker_V&_req, ignition::msgs::Boolean &_res) +{ + std::lock_guard lock(this->mutex); + std::copy(_req.marker().begin(), _req.marker().end(), + std::back_inserter(this->markerMsgs)); + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +bool MarkerManagerPrivate::ProcessMarkerMsg(const ignition::msgs::Marker &_msg) +{ + // Get the namespace, if it exists. Otherwise, use the global namespace + std::string ns; + if (!_msg.ns().empty()) { + ns = _msg.ns(); + } + + // Get the namespace that the marker belongs to + auto nsIter = this->visuals.find(ns); + + // If an id is given + size_t id; + if (_msg.id() != 0) + { + id = _msg.id(); + } + // Otherwise generate unique id + else + { + id = ignition::math::Rand::IntUniform(0, ignition::math::MAX_I32); + + // Make sure it's unique if namespace is given + if (nsIter != this->visuals.end()) + { + while (nsIter->second.find(id) != nsIter->second.end()) + id = ignition::math::Rand::IntUniform(ignition::math::MIN_UI32, + ignition::math::MAX_UI32); + } + } + + // Get visual for this namespace and id + std::map::iterator visualIter; + if (nsIter != this->visuals.end()) + visualIter = nsIter->second.find(id); + + // Add/modify a marker + if (_msg.action() == ignition::msgs::Marker::ADD_MODIFY) + { + // Modify an existing marker, identified by namespace and id + if (nsIter != this->visuals.end() && + visualIter != nsIter->second.end()) + { + if (visualIter->second->GeometryCount() > 0u) + { + // TODO(anyone): Update so that multiple markers can + // be attached to one visual + ignition::rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast + (visualIter->second->GeometryByIndex(0)); + + visualIter->second->RemoveGeometryByIndex(0); + + // Set the visual values from the Marker Message + this->SetVisual(_msg, visualIter->second); + + // Set the marker values from the Marker Message + this->SetMarker(_msg, markerPtr); + + visualIter->second->AddGeometry(markerPtr); + } + } + // Otherwise create a new marker + else + { + // Create the name for the marker + std::string name = "__IGN_MARKER_VISUAL_" + ns + "_" + + std::to_string(id); + + // Create the new marker + rendering::VisualPtr visualPtr = this->scene->CreateVisual(name); + + // Create and load the marker + rendering::MarkerPtr markerPtr = this->scene->CreateMarker(); + + // Set the visual values from the Marker Message + this->SetVisual(_msg, visualPtr); + + // Set the marker values from the Marker Message + this->SetMarker(_msg, markerPtr); + + // Add populated marker to the visual + visualPtr->AddGeometry(markerPtr); + + // Add visual to root visual + if (!visualPtr->HasParent()) + { + this->scene->RootVisual()->AddChild(visualPtr); + } + + // Store the visual + this->visuals[ns][id] = visualPtr; + } + } + // Remove a single marker + else if (_msg.action() == ignition::msgs::Marker::DELETE_MARKER) + { + // Remove the marker if it can be found. + if (nsIter != this->visuals.end() && + visualIter != nsIter->second.end()) + { + this->scene->DestroyVisual(visualIter->second); + this->visuals[ns].erase(visualIter); + + // Remove namespace if empty + if (this->visuals[ns].empty()) + this->visuals.erase(nsIter); + } + else + { + ignwarn << "Unable to delete marker with id[" << id << "] " + << "in namespace[" << ns << "]" << std::endl; + return false; + } + } + // Remove all markers, or all markers in a namespace + else if (_msg.action() == ignition::msgs::Marker::DELETE_ALL) + { + // If given namespace doesn't exist + if (!ns.empty() && nsIter == this->visuals.end()) + { + ignwarn << "Unable to delete all markers in namespace[" << ns << + "], namespace can't be found." << std::endl; + return false; + } + // Remove all markers in the specified namespace + else if (nsIter != this->visuals.end()) + { + for (auto it : nsIter->second) + { + this->scene->DestroyVisual(it.second); + } + nsIter->second.clear(); + this->visuals.erase(nsIter); + } + // Remove all markers in all namespaces. + else + { + for (nsIter = this->visuals.begin(); + nsIter != this->visuals.end(); ++nsIter) + { + for (auto it : nsIter->second) + { + this->scene->DestroyVisual(it.second); + } + } + this->visuals.clear(); + } + } + else + { + ignerr << "Unknown marker action[" << _msg.action() << "]\n"; + return false; + } + + return true; +} + +///////////////////////////////////////////////// +void MarkerManagerPrivate::SetVisual(const ignition::msgs::Marker &_msg, + const rendering::VisualPtr &_visualPtr) +{ + // Set Visual Scale + if (_msg.has_scale()) + { + _visualPtr->SetLocalScale(_msg.scale().x(), + _msg.scale().y(), + _msg.scale().z()); + } + + // Set Visual Pose + if (_msg.has_pose()) + { + math::Pose3d pose(_msg.pose().position().x(), + _msg.pose().position().y(), + _msg.pose().position().z(), + _msg.pose().orientation().w(), + _msg.pose().orientation().x(), + _msg.pose().orientation().y(), + _msg.pose().orientation().z()); + pose.Correct(); + _visualPtr->SetLocalPose(pose); + } + + // Set Visual Parent + if (!_msg.parent().empty()) + { + if (_visualPtr->HasParent()) + { + _visualPtr->Parent()->RemoveChild(_visualPtr); + } + + rendering::VisualPtr parent = this->scene->VisualByName(_msg.parent()); + + if (parent) + { + parent->AddChild(_visualPtr); + } + else + { + ignerr << "No visual with the name[" << _msg.parent() << "]\n"; + } + } + + // todo(anyone) Update Marker Visibility +} + +///////////////////////////////////////////////// +void MarkerManagerPrivate::SetMarker(const ignition::msgs::Marker &_msg, + const rendering::MarkerPtr &_markerPtr) +{ + _markerPtr->SetLayer(_msg.layer()); + + // Set Marker Lifetime + std::chrono::steady_clock::duration lifetime = + std::chrono::seconds(_msg.lifetime().sec()) + + std::chrono::nanoseconds(_msg.lifetime().nsec()); + + if (lifetime.count() != 0) + { + _markerPtr->SetLifetime(lifetime + this->simTime); + } + else + { + _markerPtr->SetLifetime(std::chrono::seconds(0)); + } + // Set Marker Render Type + ignition::rendering::MarkerType markerType = MsgToType(_msg); + _markerPtr->SetType(markerType); + + // Set Marker Material + if (_msg.has_material()) + { + rendering::MaterialPtr materialPtr = MsgToMaterial(_msg); + _markerPtr->SetMaterial(materialPtr, true /* clone */); + + // clean up material after clone + this->scene->DestroyMaterial(materialPtr); + } + + // Assume the presence of points means we clear old ones + if (_msg.point().size() > 0) + { + _markerPtr->ClearPoints(); + } + + math::Color color( + _msg.material().diffuse().r(), + _msg.material().diffuse().g(), + _msg.material().diffuse().b(), + _msg.material().diffuse().a()); + + // Set Marker Points + for (int i = 0; i < _msg.point().size(); ++i) + { + math::Vector3d vector( + _msg.point(i).x(), + _msg.point(i).y(), + _msg.point(i).z()); + + _markerPtr->AddPoint(vector, color); + } +} + +///////////////////////////////////////////////// +rendering::MaterialPtr MarkerManagerPrivate::MsgToMaterial( + const ignition::msgs::Marker &_msg) +{ + rendering::MaterialPtr material = this->scene->CreateMaterial(); + + material->SetAmbient( + _msg.material().ambient().r(), + _msg.material().ambient().g(), + _msg.material().ambient().b(), + _msg.material().ambient().a()); + + material->SetDiffuse( + _msg.material().diffuse().r(), + _msg.material().diffuse().g(), + _msg.material().diffuse().b(), + _msg.material().diffuse().a()); + + material->SetSpecular( + _msg.material().specular().r(), + _msg.material().specular().g(), + _msg.material().specular().b(), + _msg.material().specular().a()); + + material->SetEmissive( + _msg.material().emissive().r(), + _msg.material().emissive().g(), + _msg.material().emissive().b(), + _msg.material().emissive().a()); + + material->SetLightingEnabled(_msg.material().lighting()); + + return material; +} + +///////////////////////////////////////////////// +ignition::rendering::MarkerType MarkerManagerPrivate::MsgToType( + const ignition::msgs::Marker &_msg) +{ + ignition::msgs::Marker_Type marker = this->msg.type(); + if (marker != _msg.type() && _msg.type() != ignition::msgs::Marker::NONE) + { + marker = _msg.type(); + this->msg.set_type(_msg.type()); + } + switch (marker) + { + case ignition::msgs::Marker::BOX: + return ignition::rendering::MarkerType::MT_BOX; + case ignition::msgs::Marker::CAPSULE: + return ignition::rendering::MarkerType::MT_CAPSULE; + case ignition::msgs::Marker::CYLINDER: + return ignition::rendering::MarkerType::MT_CYLINDER; + case ignition::msgs::Marker::LINE_STRIP: + return ignition::rendering::MarkerType::MT_LINE_STRIP; + case ignition::msgs::Marker::LINE_LIST: + return ignition::rendering::MarkerType::MT_LINE_LIST; + case ignition::msgs::Marker::POINTS: + return ignition::rendering::MarkerType::MT_POINTS; + case ignition::msgs::Marker::SPHERE: + return ignition::rendering::MarkerType::MT_SPHERE; + case ignition::msgs::Marker::TEXT: + return ignition::rendering::MarkerType::MT_TEXT; + case ignition::msgs::Marker::TRIANGLE_FAN: + return ignition::rendering::MarkerType::MT_TRIANGLE_FAN; + case ignition::msgs::Marker::TRIANGLE_LIST: + return ignition::rendering::MarkerType::MT_TRIANGLE_LIST; + case ignition::msgs::Marker::TRIANGLE_STRIP: + return ignition::rendering::MarkerType::MT_TRIANGLE_STRIP; + default: + ignerr << "Unable to create marker of type[" << _msg.type() << "]\n"; + break; + } + return ignition::rendering::MarkerType::MT_NONE; +} + +///////////////////////////////////////////////// +void MarkerManagerPrivate::OnWorldStatsMsg( + const ignition::msgs::WorldStatistics &_msg) +{ + std::lock_guard lock(this->mutex); + std::chrono::steady_clock::duration timePoint; + if (_msg.has_sim_time()) + { + timePoint = math::secNsecToDuration( + _msg.sim_time().sec(), + _msg.sim_time().nsec()); + this->simTime = timePoint; + } + else if (_msg.has_real_time()) + { + timePoint = math::secNsecToDuration( + _msg.real_time().sec(), + _msg.real_time().nsec()); + this->simTime = timePoint; + } +} + +///////////////////////////////////////////////// +MarkerManager::MarkerManager() + : Plugin(), dataPtr(new MarkerManagerPrivate) +{ +} + +///////////////////////////////////////////////// +MarkerManager::~MarkerManager() +{ +} + +///////////////////////////////////////////////// +void MarkerManager::LoadConfig(const tinyxml2::XMLElement * _pluginElem) +{ + if (this->title.empty()) + this->title = "Marker Manager"; + + // Custom parameters + if (_pluginElem) + { + auto elem = _pluginElem->FirstChildElement("topic_name"); + if (nullptr != elem && nullptr != elem->GetText()) + { + std::string topic = transport::TopicUtils::AsValidTopic(elem->GetText()); + if (!topic.empty()) + { + this->dataPtr->topicName = topic; + } + else + { + ignerr << "the provided topic is no allowed. Using default [" + << this->dataPtr->topicName << "]"<< std::endl; + } + } + + // World name from window, to construct default topics and services + std::string worldName = "example"; + auto worldNames = gui::worldNames(); + if (!worldNames.empty()) + worldName = worldNames[0].toStdString(); + + // Subscribe to world stats + std::string statsTopic; + auto statsTopicElem = _pluginElem->FirstChildElement("stats_topic"); + if (nullptr != statsTopicElem && nullptr != statsTopicElem->GetText()) + statsTopic = statsTopicElem->GetText(); + + // Service specified with different world name + auto parts = common::Split(statsTopic, '/'); + if (!worldName.empty() && + parts.size() == 4 && + parts[0] == "" && + parts[1] == "world" && + parts[2] != worldName && + parts[3] == "stats") + { + ignwarn << "Ignoring topic [" << statsTopic + << "], world name different from [" << worldName + << "]. Fix or remove your tag." << std::endl; + + statsTopic = "/world/" + worldName + "/stats"; + } + + if (statsTopic.empty() && !worldName.empty()) + { + statsTopic = "/world/" + worldName + "/stats"; + } + + statsTopic = transport::TopicUtils::AsValidTopic(statsTopic); + if (!statsTopic.empty()) + { + // Subscribe to world_stats + if (!this->dataPtr->node.Subscribe(statsTopic, + &MarkerManagerPrivate::OnWorldStatsMsg, this->dataPtr.get())) + { + ignerr << "Failed to subscribe to [" << statsTopic << "]" << std::endl; + } + else + { + ignmsg << "Listening to stats on [" << statsTopic << "]" << std::endl; + } + } + else + { + ignerr << "Failed to create valid topic for world [" << worldName << "]" + << std::endl; + } + } + + App()->findChild()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool MarkerManager::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == events::Render::kType) + { + this->dataPtr->OnRender(); + } + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gui::plugins::MarkerManager, + ignition::gui::Plugin) diff --git a/src/plugins/marker_manager/MarkerManager.hh b/src/plugins/marker_manager/MarkerManager.hh new file mode 100644 index 000000000..4488fe14f --- /dev/null +++ b/src/plugins/marker_manager/MarkerManager.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GUI_PLUGINS_MARKERMANAGER_HH_ +#define IGNITION_GUI_PLUGINS_MARKERMANAGER_HH_ + +#include + +#include "ignition/gui/Plugin.hh" + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + class MarkerManagerPrivate; + + /// \brief This plugin will be in charge of handeling the markers in the + /// scene. It will allow to add, modify or remove markers. + class MarkerManager : public Plugin + { + Q_OBJECT + + /// \brief Constructor + public: MarkerManager(); + + /// \brief Destructor + public: virtual ~MarkerManager(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // 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/marker_manager/MarkerManager.qml b/src/plugins/marker_manager/MarkerManager.qml new file mode 100644 index 000000000..873da3001 --- /dev/null +++ b/src/plugins/marker_manager/MarkerManager.qml @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +import QtQuick 2.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/plugins/marker_manager/MarkerManager.qrc b/src/plugins/marker_manager/MarkerManager.qrc new file mode 100644 index 000000000..e37e40426 --- /dev/null +++ b/src/plugins/marker_manager/MarkerManager.qrc @@ -0,0 +1,5 @@ + + + MarkerManager.qml + + diff --git a/test/integration/marker_manager.cc b/test/integration/marker_manager.cc new file mode 100644 index 000000000..2f1c1c79b --- /dev/null +++ b/test/integration/marker_manager.cc @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include +#include + +#include +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "ignition/gui/Application.hh" +#include "ignition/gui/GuiEvents.hh" +#include "ignition/gui/MainWindow.hh" +#include "ignition/gui/Plugin.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./MarkerMmanager_TEST")), +}; + +using namespace std::chrono_literals; + +using namespace ignition; +using namespace gui; + +class MarkerManagerTestFixture : public ::testing::Test +{ + + public: + ignition::transport::Node node; + rendering::ScenePtr scene; + ignition::transport::Node::Publisher statsPub; + + MarkerManagerTestFixture() + { + // Periodic world statistics + statsPub = + node.Advertise("/example/stats"); + } + + void sendWorldStatisticsMsg(std::chrono::steady_clock::duration &timePoint) + { + ignition::msgs::WorldStatistics msgWorldStatistics; + + msgWorldStatistics.set_real_time_factor(1); + auto s = std::chrono::duration_cast(timePoint); + auto ns = + std::chrono::duration_cast(timePoint-s); + + msgWorldStatistics.mutable_sim_time()->set_sec(s.count()); + msgWorldStatistics.mutable_sim_time()->set_nsec(ns.count()); + statsPub.Publish(msgWorldStatistics); + } + + void waitAndSendStatsMsgs( + std::chrono::steady_clock::duration &timePoint, + uint expectedValue, + int maxSleep) + { + + // Give it time to be processed + int sleep = 0; + while (scene->VisualCount() != expectedValue && sleep < maxSleep) + { + timePoint += 100ms; + + sendWorldStatisticsMsg(timePoint); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + } +}; + +///////////////////////////////////////////////// +TEST_F(MarkerManagerTestFixture, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(MarkerManager)) +{ + common::Console::SetVerbosity(4); + + // Load the plugin + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + // Load plugin + const char *pluginStr = + "" + "/example/stats" + ""; + + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + + EXPECT_TRUE(app.LoadPlugin("MarkerManager", + pluginDoc.FirstChildElement("plugin"))); + EXPECT_TRUE(app.LoadPlugin("MinimalScene")); + + // Get main window + auto window = app.findChild(); + ASSERT_NE(window, nullptr); + + // Get plugin + auto plugins = window->findChildren(); + EXPECT_EQ(plugins.size(), 2); + + // Show, but don't exec, so we don't block + window->QuickWindow()->show(); + + // Check scene + auto engine = rendering::engine("ogre"); + ASSERT_NE(nullptr, engine); + + int sleep = 0; + int maxSleep = 30; + while (0 == engine->SceneCount() && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_EQ(1u, engine->SceneCount()); + scene = engine->SceneByName("scene"); + ASSERT_NE(nullptr, scene); + + std::chrono::steady_clock::duration timePoint = + std::chrono::steady_clock::duration::zero(); + + // Create the marker message + ignition::msgs::Marker markerMsg; + ignition::msgs::Material matMsg; + markerMsg.set_ns("default"); + markerMsg.set_id(0); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(ignition::msgs::Marker::SPHERE); + markerMsg.set_visibility(ignition::msgs::Marker::GUI); + + // Add a sphere that will be remove after 2 seconds + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(1); + markerMsg.mutable_material()->mutable_ambient()->set_a(1); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(1); + markerMsg.mutable_material()->mutable_diffuse()->set_a(1); + markerMsg.mutable_lifetime()->set_sec(0); + markerMsg.mutable_lifetime()->set_nsec(0); + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(1.0, 1.0, 1.0)); + + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(2, 2, 0, 0, 0, 0)); + EXPECT_EQ(0u, scene->VisualCount()); + + // Wait 2 seconds, plugins need to be initialized + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + bool executed = node.Request("/marker", markerMsg); + if (executed) + { + igndbg << "/marker request sent" << std::endl; + waitAndSendStatsMsgs(timePoint, 1, 200); + EXPECT_EQ(1u, scene->VisualCount()); + } + else + { + FAIL(); + } + + markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + executed = node.Request("/marker", markerMsg); + if (executed) + { + waitAndSendStatsMsgs(timePoint, 0, 200); + EXPECT_EQ(0u, scene->VisualCount()); + } + else + { + FAIL(); + } + + // Cleanup + plugins.clear(); +}