diff --git a/Migration.md b/Migration.md index 0dd161b80f..7fdc24cfcb 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,14 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Gazebo 11.14 to 11.15 + +### Modifications + +The `gz world` tool now publishes a WorldControl message using gz-transport, +and the `physics::World` class and other subscribers now subscribe to both +`gz-transport` (ZeroMQ) and `gazebo_transport` (boost asio) topics. + ## Gazebo 11.2 to 11.3 ### Modifications diff --git a/gazebo/gui/plot/PlotManager.cc b/gazebo/gui/plot/PlotManager.cc index 634b19e406..244ca11b29 100644 --- a/gazebo/gui/plot/PlotManager.cc +++ b/gazebo/gui/plot/PlotManager.cc @@ -18,6 +18,7 @@ #include #include +#include #include "gazebo/msgs/msgs.hh" #include "gazebo/transport/TransportIface.hh" @@ -50,6 +51,9 @@ namespace gazebo /// \brief Subscriber to the world control topic public: transport::SubscriberPtr worldControlSub; + /// \brief Node for ignition transport communication. + public: ignition::transport::Node ignNode; + /// \brief Handler for updating introspection curves public: IntrospectionCurveHandler introspectionCurve; @@ -73,6 +77,15 @@ PlotManager::PlotManager() this->dataPtr->worldControlSub = this->dataPtr->node->Subscribe("~/world_control", &PlotManager::OnWorldControl, this); + { + // Also subscribe to WorldControl messages over ZeroMQ-based gz-transport + std::string worldControlTopic("/world_control"); + if (!this->dataPtr->ignNode.Subscribe(worldControlTopic, + &PlotManager::OnControl, this)) + { + gzerr << "Error advertising topic [" << worldControlTopic << "]\n"; + } + } } ///////////////////////////////////////////////// @@ -84,10 +97,16 @@ PlotManager::~PlotManager() ///////////////////////////////////////////////// void PlotManager::OnWorldControl(ConstWorldControlPtr &_data) { - if (_data->has_reset()) + this->OnControl(*_data); +} + +///////////////////////////////////////////////// +void PlotManager::OnControl(const msgs::WorldControl &_data) +{ + if (_data.has_reset()) { - if ((_data->reset().has_all() && _data->reset().all()) || - (_data->reset().has_time_only() && _data->reset().time_only())) + if ((_data.reset().has_all() && _data.reset().all()) || + (_data.reset().has_time_only() && _data.reset().time_only())) { std::lock_guard lock(this->dataPtr->mutex); for (auto &w : this->dataPtr->windows) diff --git a/gazebo/gui/plot/PlotManager.hh b/gazebo/gui/plot/PlotManager.hh index 29101fca76..e851d5a999 100644 --- a/gazebo/gui/plot/PlotManager.hh +++ b/gazebo/gui/plot/PlotManager.hh @@ -46,7 +46,8 @@ namespace gazebo /// \brief Destructor. public: virtual ~PlotManager(); - /// \brief Callback when a world control message is received. It is used + /// \brief Callback when a world control message is received over + /// gazebo_transport using boost asio. It is used /// to detect simulation resets. /// \param[in] _data Message data containing world control commands public: void OnWorldControl(ConstWorldControlPtr &_data); @@ -90,6 +91,11 @@ namespace gazebo /// \brief Returns a pointer to the unique (static) instance public: static PlotManager* Instance(); + /// \brief Callback when a world control message is received over + /// gz-transport using ZeroMQ. It is used to detect simulation resets. + /// \param[in] _data Message data containing world control commands + private: void OnControl(const msgs::WorldControl &_data); + /// \brief This is a singleton class. private: friend class SingletonT; diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 714be31478..6d36124fb0 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -270,6 +270,15 @@ void World::Load(sdf::ElementPtr _sdf) &World::OnFactoryMsg, this); this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control", &World::OnControl, this); + { + // Also subscribe to WorldControl messages over ZeroMQ-based gz-transport + std::string worldControlTopic("/world_control"); + if (!this->dataPtr->ignNode.Subscribe(worldControlTopic, + &World::OnWorldControl, this)) + { + gzerr << "Error advertising topic [" << worldControlTopic << "]\n"; + } + } this->dataPtr->playbackControlSub = this->dataPtr->node->Subscribe( "~/playback_control", &World::OnPlaybackControl, this); @@ -1609,32 +1618,38 @@ void World::OnFactoryMsg(ConstFactoryPtr &_msg) ////////////////////////////////////////////////// void World::OnControl(ConstWorldControlPtr &_data) { - if (_data->has_pause()) - this->SetPaused(_data->pause()); + this->OnWorldControl(*_data); +} + +////////////////////////////////////////////////// +void World::OnWorldControl(const msgs::WorldControl &_data) +{ + if (_data.has_pause()) + this->SetPaused(_data.pause()); - if (_data->has_step()) + if (_data.has_step()) this->OnStep(); - if (_data->has_multi_step()) + if (_data.has_multi_step()) { // stepWorld is a blocking call so set stepInc directly so that world stats // will still be published this->SetPaused(true); std::lock_guard lock(this->dataPtr->worldUpdateMutex); - this->dataPtr->stepInc = _data->multi_step(); + this->dataPtr->stepInc = _data.multi_step(); } - if (_data->has_seed()) + if (_data.has_seed()) { - ignition::math::Rand::Seed(_data->seed()); - this->dataPtr->physicsEngine->SetSeed(_data->seed()); + ignition::math::Rand::Seed(_data.seed()); + this->dataPtr->physicsEngine->SetSeed(_data.seed()); } - if (_data->has_reset()) + if (_data.has_reset()) { this->dataPtr->needsReset = true; - if (_data->reset().has_all() && _data->reset().all()) + if (_data.reset().has_all() && _data.reset().all()) { this->dataPtr->resetAll = true; } @@ -1642,10 +1657,10 @@ void World::OnControl(ConstWorldControlPtr &_data) { this->dataPtr->resetAll = false; - if (_data->reset().has_time_only() && _data->reset().time_only()) + if (_data.reset().has_time_only() && _data.reset().time_only()) this->dataPtr->resetTimeOnly = true; - if (_data->reset().has_model_only() && _data->reset().model_only()) + if (_data.reset().has_model_only() && _data.reset().model_only()) this->dataPtr->resetModelOnly = true; } } diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 2284318f01..c268e84aa7 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -538,10 +538,16 @@ namespace gazebo /// \brief Step callback. private: void OnStep(); - /// \brief Called when a world control message is received. + /// \brief Called when a world control message is received on the + /// gazebo_transport topic using boost asio. /// \param[in] _data The world control message. private: void OnControl(ConstWorldControlPtr &_data); + /// \brief Called when a world control message is received on the + /// gz-transport topic using ZeroMQ. + /// \param[in] _data The world control message. + private: void OnWorldControl(const msgs::WorldControl &_data); + /// \brief Called when log playback control message is received. /// \param[in] _data The log playback control message. private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data); diff --git a/gazebo/rendering/OculusCamera.cc b/gazebo/rendering/OculusCamera.cc index 7151d98d95..c6bc05845b 100644 --- a/gazebo/rendering/OculusCamera.cc +++ b/gazebo/rendering/OculusCamera.cc @@ -130,6 +130,15 @@ OculusCamera::OculusCamera(const std::string &_name, ScenePtr _scene) this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control", &OculusCamera::OnControl, this); + { + // Also subscribe to WorldControl messages over ZeroMQ-based gz-transport + std::string worldControlTopic("/world_control"); + if (!this->dataPtr->ignNode.Subscribe(worldControlTopic, + &OculusCamera::OnWorldControl, this)) + { + gzerr << "Error advertising topic [" << worldControlTopic << "]\n"; + } + } // Oculus is now ready. this->dataPtr->ready = true; @@ -162,7 +171,13 @@ void OculusCamera::Load(sdf::ElementPtr _sdf) ////////////////////////////////////////////////// void OculusCamera::OnControl(ConstWorldControlPtr &_data) { - if (_data->has_reset() && _data->reset().has_all() && _data->reset().all()) + this->OnWorldControl(*_data); +} + +////////////////////////////////////////////////// +void OculusCamera::OnWorldControl(const msgs::WorldControl &_data) +{ + if (_data.has_reset() && _data.reset().has_all() && _data.reset().all()) { this->ResetSensor(); } diff --git a/gazebo/rendering/OculusCamera.hh b/gazebo/rendering/OculusCamera.hh index fc8bba83a3..d6f94dd081 100644 --- a/gazebo/rendering/OculusCamera.hh +++ b/gazebo/rendering/OculusCamera.hh @@ -123,10 +123,14 @@ namespace gazebo /// \return True if the camera is now tracking the visual. protected: virtual bool TrackVisualImpl(VisualPtr _visual); - /// \brief Receive world control messages. Used to reset the oculus - /// sensor. + /// \brief Receive world control messages over gazebo_transport using + /// boost asio. Used to reset the oculus sensor. private: void OnControl(ConstWorldControlPtr &_data); + /// \brief Receive world control messages over gz-transport using + /// ZeroMQ. Used to reset the oculus sensor. + private: void OnWorldControl(const msgs::WorldControl &_data); + /// \brief Apply distorsion to the render target. private: void Oculus(); diff --git a/gazebo/rendering/OculusCameraPrivate.hh b/gazebo/rendering/OculusCameraPrivate.hh index c420e15038..5e7d6bdfe4 100644 --- a/gazebo/rendering/OculusCameraPrivate.hh +++ b/gazebo/rendering/OculusCameraPrivate.hh @@ -18,6 +18,7 @@ #define _GAZEBO_RENDERING_OCULUS_CAMERA_PRIVATE_HH_ #include +#include #include "gazebo/util/system.hh" namespace Ogre @@ -73,6 +74,9 @@ namespace gazebo /// \brief Subscriber used to receive updates on world_control topic. public: transport::SubscriberPtr controlSub; + /// \brief Node for ignition transport communication. + public: ignition::transport::Node ignNode; + /// \brief True when Oculus is connected and ready to use. public: bool ready; diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 94a3bb0e38..6a724f68db 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -37,5 +37,5 @@ if (NOT APPLE AND NOT WIN32) set(tool_tests gz_stress.cc ) - gz_build_tests(${tool_tests} EXTRA_LIBS gazebo_transport) + gz_build_tests(${tool_tests} EXTRA_LIBS gazebo_transport ${IGNITION-TRANSPORT_LIBRARIES}) endif() diff --git a/test/performance/gz_stress.cc b/test/performance/gz_stress.cc index 3340044971..8b15e6ed8d 100644 --- a/test/performance/gz_stress.cc +++ b/test/performance/gz_stress.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -40,10 +41,10 @@ pid_t g_pid = -1; boost::condition_variable g_msgCondition; ///////////////////////////////////////////////// -void WorldControlCB(ConstWorldControlPtr &_msg) +void WorldControlCB(const msgs::WorldControl &_msg) { boost::mutex::scoped_lock lock(g_mutex); - g_msgDebugOut = _msg->DebugString(); + g_msgDebugOut = _msg.DebugString(); g_msgCondition.notify_all(); } @@ -149,10 +150,8 @@ TEST_F(gzTest, Stress) { init(); - gazebo::transport::NodePtr node(new gazebo::transport::Node()); - node->Init(); - gazebo::transport::SubscriberPtr sub = - node->Subscribe("~/world_control", &WorldControlCB, true); + ignition::transport::Node ignNode; + ignNode.Subscribe("/world_control", &WorldControlCB); // Run the transport loop: starts a new thread gazebo::transport::run(); diff --git a/tools/gz.cc b/tools/gz.cc index 7a8f3d6b16..8e54c01d27 100644 --- a/tools/gz.cc +++ b/tools/gz.cc @@ -238,12 +238,25 @@ bool WorldCommand::RunImpl() if (this->vm.count("world-name")) worldName = this->vm["world-name"].as(); - transport::NodePtr node(new transport::Node()); - node->Init(worldName); + ignition::transport::Node ignNode; - transport::PublisherPtr pub = - node->Advertise("~/world_control"); - pub->WaitForConnection(); + const std::string topic{"/world_control"}; + auto pub = ignNode.Advertise(topic); + + // Wait for subscribers + unsigned int maxSleep = 30; + unsigned int sleep = 0; + unsigned int mSleep = 100; + for (; sleep < maxSleep && !pub.HasConnections(); ++sleep) + { + common::Time::MSleep(mSleep); + } + if (sleep == maxSleep) + { + gzerr << "No subscribers to topic [" << topic <<"], timed out after " << + maxSleep * mSleep << "ms." << std::endl; + return false; + } msgs::WorldControl msg; bool good = false; @@ -285,9 +298,13 @@ bool WorldCommand::RunImpl() } if (good) - pub->Publish(msg, true); + { + pub.Publish(msg); + } else + { this->Help(); + } return true; } diff --git a/tools/gz_TEST.cc b/tools/gz_TEST.cc index 130c250848..a73e1f48a9 100644 --- a/tools/gz_TEST.cc +++ b/tools/gz_TEST.cc @@ -190,10 +190,10 @@ void FactoryCB(ConstFactoryPtr &_msg) } ///////////////////////////////////////////////// -void WorldControlCB(ConstWorldControlPtr &_msg) +void WorldControlCB(const msgs::WorldControl &_msg) { boost::mutex::scoped_lock lock(g_mutex); - g_msgDebugOut = _msg->DebugString(); + g_msgDebugOut = _msg.DebugString(); g_msgCondition.notify_all(); } @@ -503,10 +503,8 @@ TEST_F(gzTest, World) std::string helpOutput = custom_exec_str("gz help world"); EXPECT_NE(helpOutput.find("gz world"), std::string::npos); - gazebo::transport::NodePtr node(new gazebo::transport::Node()); - node->Init(); - gazebo::transport::SubscriberPtr sub = - node->Subscribe("~/world_control", &WorldControlCB); + ignition::transport::Node ignNode; + ignNode.Subscribe("/world_control", &WorldControlCB); // Test world pause {