From 9cc9fc217e4030ba1b77389a480c7219b33989ef Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Sep 2021 00:17:31 -0700 Subject: [PATCH 1/5] World: advertise scene info as ignition service Add test for /scene_info service with empty.world Signed-off-by: Steve Peters --- gazebo/physics/World.cc | 29 +++++++++++++++++++++++++++++ gazebo/physics/World.hh | 5 +++++ test/integration/info_services.cc | 30 ++++++++++++++++++++++++++++++ 3 files changed, 64 insertions(+) diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index e396eecb52..1d48c3f659 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -296,6 +296,14 @@ void World::Load(sdf::ElementPtr _sdf) << std::endl; } + std::string sceneInfoService("/scene_info"); + if (!this->dataPtr->ignNode.Advertise(sceneInfoService, + &World::SceneInfoService, this)) + { + gzerr << "Error advertising service [" << sceneInfoService << "]" + << std::endl; + } + std::string shadowCasterService("/shadow_caster_material_name"); if (!this->dataPtr->ignNode.Advertise(shadowCasterService, &World::ShadowCasterService, this)) @@ -3372,6 +3380,27 @@ bool World::PluginInfoService(const ignition::msgs::StringMsg &_req, return false; } +////////////////////////////////////////////////// +bool World::SceneInfoService(msgs::Scene &_res) +{ + std::lock_guard lock(this->dataPtr->receiveMutex); + + // Copy implementation from ProcessRequestMsgs + this->dataPtr->sceneMsg.clear_model(); + this->dataPtr->sceneMsg.clear_light(); + this->BuildSceneMsg(this->dataPtr->sceneMsg, this->dataPtr->rootElement); + + _res = this->dataPtr->sceneMsg; + + for (auto road : this->dataPtr->roads) + { + // this causes the roads to publish road msgs. + road->Init(); + } + + return true; +} + ////////////////////////////////////////////////// bool World::ShadowCasterService(ignition::msgs::StringMsg &_res) { diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index 1f5dacf1d5..3e4dc85048 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -651,6 +651,11 @@ namespace gazebo private: bool PluginInfoService(const ignition::msgs::StringMsg &_request, ignition::msgs::Plugin_V &_plugins); + /// \brief Callback for "/scene_info" service. + /// \param[out] _response Message containing scene info + /// \return True if the info was successfully obtained. + private: bool SceneInfoService(msgs::Scene &_response); + /// \brief Callback for "/shadow_caster_material_name" service. /// \param[out] _response Message containing shadow caster material name /// \return True if the info was successfully obtained. diff --git a/test/integration/info_services.cc b/test/integration/info_services.cc index 3f26c19766..619b866f7b 100644 --- a/test/integration/info_services.cc +++ b/test/integration/info_services.cc @@ -204,6 +204,36 @@ TEST_F(InfoServicesTest, ModelPlugins) EXPECT_TRUE(g_badElement); } +///////////////////////////////////////////////// +// Request info about scene +TEST_F(InfoServicesTest, Scene) +{ + this->Load("worlds/empty.world"); + + ignition::transport::Node ignNode; + + std::string sceneInfoService("/scene_info"); + + std::vector publishers; + ASSERT_TRUE(ignNode.ServiceInfo(sceneInfoService, publishers)); + + gazebo::msgs::Scene scene; + unsigned int timeout = 5000; + bool result; + EXPECT_TRUE(ignNode.Request(sceneInfoService, timeout, scene, result)); + EXPECT_TRUE(result); + + EXPECT_EQ("default", scene.name()); + + ASSERT_EQ(1, scene.model_size()); + auto model = scene.model(0); + EXPECT_EQ("ground_plane", model.name()); + + ASSERT_EQ(1, scene.light_size()); + auto light = scene.light(0); + EXPECT_EQ("sun", light.name()); +} + ///////////////////////////////////////////////// // Main int main(int argc, char **argv) From 80a64f9d6e59e1ea5b2c840173b367001ea2cf95 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Sep 2021 00:18:01 -0700 Subject: [PATCH 2/5] Scene: use ignition service for scene info This should be more reliable than the ~/request topic over gazebo::transport, but if it fails then fall back on the current method. Signed-off-by: Steve Peters --- gazebo/rendering/Scene.cc | 37 +++++++++++++++++++++++++++++++------ gazebo/rendering/Scene.hh | 6 ++++++ 2 files changed, 37 insertions(+), 6 deletions(-) diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 02bfcd5a0b..960348622a 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -430,9 +430,21 @@ void Scene::Init() this->dataPtr->worldVisual)); this->dataPtr->originVisual->Load(); - this->dataPtr->requestPub->WaitForConnection(); - 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; + ignition::msgs::Scene sceneInfo; + const std::string serviceName = "/scene_info"; + std::vector publishers; + if (!node.ServiceInfo(serviceName, publishers) || + !node.Request(serviceName, &Scene::OnSceneInfo, this)) + { + gzwarn << "Ignition transport [" << serviceName << "] service call failed," + << " falling back to gazebo transport [scene_info] request." + << std::endl; + this->dataPtr->requestPub->WaitForConnection(); + this->dataPtr->requestMsg = msgs::CreateRequest("scene_info"); + this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg); + } if (!this->dataPtr->isServer) { @@ -2458,6 +2470,21 @@ void Scene::OnScene(ConstScenePtr &_msg) this->dataPtr->sceneMsgs.push_back(_msg); } +///////////////////////////////////////////////// +void Scene::OnSceneInfo(const msgs::Scene &_msg, const bool _result) +{ + if (_result) + { + std::lock_guard lock(*this->dataPtr->receiveMutex); + auto msgptr = boost::make_shared(_msg); + this->dataPtr->sceneMsgs.push_back(msgptr); + } + else + { + gzerr << "Error when calling /scene_info service" << std::endl; + } +} + ///////////////////////////////////////////////// void Scene::OnResponse(ConstResponsePtr &_msg) { @@ -2467,10 +2494,8 @@ void Scene::OnResponse(ConstResponsePtr &_msg) msgs::Scene sceneMsg; sceneMsg.ParseFromString(_msg->serialized_data()); - boost::shared_ptr sm(new msgs::Scene(sceneMsg)); + this->OnSceneInfo(sceneMsg, true); - std::lock_guard lock(*this->dataPtr->receiveMutex); - this->dataPtr->sceneMsgs.push_back(sm); this->dataPtr->requestMsg = NULL; } diff --git a/gazebo/rendering/Scene.hh b/gazebo/rendering/Scene.hh index 40e1320192..75280fdec7 100644 --- a/gazebo/rendering/Scene.hh +++ b/gazebo/rendering/Scene.hh @@ -657,6 +657,12 @@ namespace gazebo /// \param[in] _msg The message. private: void OnScene(ConstScenePtr &_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); + /// \brief Response callback /// \param[in] _msg The message data. private: void OnResponse(ConstResponsePtr &_msg); From fe075b5954fb2c97a8bf180ae876572eb4d6348a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 11 Oct 2021 22:16:28 -0700 Subject: [PATCH 3/5] ScenePrivate: store requestMsg as unique_ptr Signed-off-by: Steve Peters --- gazebo/rendering/Scene.cc | 8 +++----- gazebo/rendering/ScenePrivate.hh | 7 ++++--- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 960348622a..398fb30765 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -123,7 +123,6 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, this->dataPtr->transparent = false; this->dataPtr->wireframe = false; - this->dataPtr->requestMsg = NULL; this->dataPtr->enableVisualizations = _enableVisualizations; this->dataPtr->node = transport::NodePtr(new transport::Node()); this->dataPtr->node->Init(_name); @@ -331,8 +330,7 @@ Scene::~Scene() { this->Clear(); - delete this->dataPtr->requestMsg; - this->dataPtr->requestMsg = NULL; + this->dataPtr->requestMsg.reset(nullptr); delete this->dataPtr->receiveMutex; this->dataPtr->receiveMutex = NULL; @@ -442,7 +440,7 @@ void Scene::Init() << " falling back to gazebo transport [scene_info] request." << std::endl; this->dataPtr->requestPub->WaitForConnection(); - this->dataPtr->requestMsg = msgs::CreateRequest("scene_info"); + this->dataPtr->requestMsg.reset(msgs::CreateRequest("scene_info")); this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg); } @@ -2496,7 +2494,7 @@ void Scene::OnResponse(ConstResponsePtr &_msg) sceneMsg.ParseFromString(_msg->serialized_data()); this->OnSceneInfo(sceneMsg, true); - this->dataPtr->requestMsg = NULL; + this->dataPtr->requestMsg.reset(nullptr); } ///////////////////////////////////////////////// diff --git a/gazebo/rendering/ScenePrivate.hh b/gazebo/rendering/ScenePrivate.hh index 11e633c4b5..73f0c850c8 100644 --- a/gazebo/rendering/ScenePrivate.hh +++ b/gazebo/rendering/ScenePrivate.hh @@ -17,12 +17,13 @@ #ifndef GAZEBO_RENDERING_SCENE_PRIVATE_HH_ #define GAZEBO_RENDERING_SCENE_PRIVATE_HH_ +#include #include #include +#include +#include #include #include -#include -#include #include @@ -300,7 +301,7 @@ namespace gazebo public: std::string selectionMode; /// \brief Keep around our request message. - public: msgs::Request *requestMsg = nullptr; + public: std::unique_ptr requestMsg; /// \brief True if visualizations should be rendered. public: bool enableVisualizations; From ab570176f67e707f2c6e6ee1c22291da598794af Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 14 Oct 2021 01:31:23 -0700 Subject: [PATCH 4/5] Fix more compiler warnings Signed-off-by: Steve Peters --- gazebo/physics/dart/DARTJointPrivate.hh | 4 ++++ tools/gz_marker.cc | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/gazebo/physics/dart/DARTJointPrivate.hh b/gazebo/physics/dart/DARTJointPrivate.hh index 73ba4931c0..04d5e3cc2a 100644 --- a/gazebo/physics/dart/DARTJointPrivate.hh +++ b/gazebo/physics/dart/DARTJointPrivate.hh @@ -76,7 +76,11 @@ namespace gazebo mFuncs.clear(); +#if DART_VERSION_AT_LEAST(6, 10, 0) + this->dtJoint->setLimitEnforcement(true); + #else this->dtJoint->setPositionLimitEnforced(true); + #endif } /// \brief Return true if DART Joint is initialized diff --git a/tools/gz_marker.cc b/tools/gz_marker.cc index a3fd4a4070..4df2e74d3b 100644 --- a/tools/gz_marker.cc +++ b/tools/gz_marker.cc @@ -230,7 +230,7 @@ void MarkerCommand::List() for (auto const &d : data) { std::cout << "NAMESPACE " << d.first << std::endl; - for (auto const m : d.second) + for (auto const &m : d.second) { uint64_t id = std::get<0>(m); std::cout << " ID " << id; From 37556f27f3e57b59e79b4af8e77ecf76e0ad8499 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 15 Oct 2021 15:01:23 -0700 Subject: [PATCH 5/5] remove unused variable Signed-off-by: Steve Peters --- gazebo/rendering/Scene.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 4cba0fb935..33ef0d31d5 100644 --- a/gazebo/rendering/Scene.cc +++ b/gazebo/rendering/Scene.cc @@ -454,7 +454,6 @@ void Scene::Init() // Get scene info from physics::World with ignition transport service ignition::transport::Node node; - ignition::msgs::Scene sceneInfo; const std::string serviceName = "/scene_info"; std::vector publishers; if (!node.ServiceInfo(serviceName, publishers) ||