MainWindow: call service to get scene_info

Try getting scene info from the /scene_info service, which is more reliable than the request / response topics. Fall back to using the request / response topics if the service is not available or the call fails. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Aug 11, 2023 · 9bbd1ad · 9bbd1ad
1 parent fae595f
commit 9bbd1ad
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 6 deletions.
diff --git a/gazebo/gui/MainWindow.cc b/gazebo/gui/MainWindow.cc
@@ -18,6 +18,7 @@
 #include <functional>
 
 #include <ignition/math/Pose3.hh>
+#include <ignition/transport/Node.hh>
 #include <sdf/sdf.hh>
 #include <boost/algorithm/string.hpp>
 
@@ -362,8 +363,19 @@ void MainWindow::Init()
                                             "/gazebo/world/modify",
                                             &MainWindow::OnWorldModify, this);
 
-  this->dataPtr->requestMsg = msgs::CreateRequest("scene_info");
-  this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg);
+  // Get scene info from physics::World with ignition transport service
+  ignition::transport::Node node;
+  const std::string serviceName = "/scene_info";
+  std::vector<ignition::transport::ServicePublisher> publishers;
+  if (!node.ServiceInfo(serviceName, publishers) ||
+      !node.Request(serviceName, &MainWindow::OnSceneInfo, this))
+  {
+    gzwarn << "Ignition transport [" << serviceName << "] service call failed,"
+           << " falling back to gazebo transport [scene_info] request."
+           << std::endl;
+    this->dataPtr->requestMsg = msgs::CreateRequest("scene_info");
+    this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg);
+  }
 
   gui::Events::mainWindowReady();
 }
@@ -2150,8 +2162,19 @@ void MainWindow::OnResponse(ConstResponsePtr &_msg)
 
   if (_msg->has_type() && _msg->type() == sceneMsg.GetTypeName())
   {
-    sceneMsg.ParseFromString(_msg->serialized_data());
+    bool parseResult = sceneMsg.ParseFromString(_msg->serialized_data());
+    this->OnSceneInfo(sceneMsg, parseResult);
+  }
+
+  delete this->dataPtr->requestMsg;
+  this->dataPtr->requestMsg = nullptr;
+}
 
+/////////////////////////////////////////////////
+void MainWindow::OnSceneInfo(const msgs::Scene &sceneMsg, const bool _result)
+{
+  if (_result)
+  {
     for (int i = 0; i < sceneMsg.model_size(); ++i)
     {
       this->dataPtr->entities[sceneMsg.model(i).name()] =
@@ -2177,9 +2200,10 @@ void MainWindow::OnResponse(ConstResponsePtr &_msg)
       gui::Events::lightUpdate(sceneMsg.light(i));
     }
   }
-
-  delete this->dataPtr->requestMsg;
-  this->dataPtr->requestMsg = nullptr;
+  else
+  {
+    gzerr << "Error when requesting scene_info" << std::endl;
+  }
 }
 
 /////////////////////////////////////////////////

diff --git a/gazebo/gui/MainWindow.hh b/gazebo/gui/MainWindow.hh
@@ -285,6 +285,12 @@ namespace gazebo
       /// \param[in] _msg Pointer to the light message.
       private: void OnLight(ConstLightPtr &_msg);
 
+      /// \brief Called when the scene info service replies with the scene
+      /// message.
+      /// \param[in] _msg The message.
+      /// \param[in] _result Flag indicating if service call succeeded.
+      private: void OnSceneInfo(const msgs::Scene &_msg, const bool _result);
+
       private: void OnResponse(ConstResponsePtr &_msg);
       private: void OnWorldModify(ConstWorldModifyPtr &_msg);
       private: void OnManipMode(const std::string &_mode);