diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc index 50144dcf81..668984aa08 100644 --- a/gazebo/physics/World.cc +++ b/gazebo/physics/World.cc @@ -300,11 +300,19 @@ 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 shadowCasterMaterialNameService("/shadow_caster_material_name"); if (!this->dataPtr->ignNode.Advertise(shadowCasterMaterialNameService, &World::ShadowCasterMaterialNameService, this)) { - gzerr << "Error advertising service [" << + gzerr << "Error advertising service [" << shadowCasterMaterialNameService << "]" << std::endl; } @@ -313,7 +321,7 @@ void World::Load(sdf::ElementPtr _sdf) if (!this->dataPtr->ignNode.Advertise(shadowCasterRenderBackFacesService, &World::ShadowCasterRenderBackFacesService, this)) { - gzerr << "Error advertising service [" << + gzerr << "Error advertising service [" << shadowCasterRenderBackFacesService << "]" << std::endl; } @@ -3385,6 +3393,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::ShadowCasterMaterialNameService(ignition::msgs::StringMsg &_res) { diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh index ef4a4b7c01..a4eedb5550 100644 --- a/gazebo/physics/World.hh +++ b/gazebo/physics/World.hh @@ -652,13 +652,18 @@ 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. private: bool ShadowCasterMaterialNameService( ignition::msgs::StringMsg &_response); - /// \brief Callback for "/shadow_caster_render_back_faces" + /// \brief Callback for "/shadow_caster_render_back_faces" /// service. /// \param[out] _response Message containing shadow caster render back /// faces 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/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc index 455e92a142..33ef0d31d5 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); @@ -226,7 +225,7 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, else { gzerr << "Service call[" << serviceName << "] timed out" << std::endl; - } + } } { @@ -248,7 +247,7 @@ Scene::Scene(const std::string &_name, const bool _enableVisualizations, else { gzerr << "Service call[" << serviceName << "] timed out" << std::endl; - } + } } } @@ -355,8 +354,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; @@ -454,9 +452,20 @@ 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; + 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.reset(msgs::CreateRequest("scene_info")); + this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg); + } if (!this->dataPtr->isServer) { @@ -2482,6 +2491,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) { @@ -2491,11 +2515,9 @@ 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; + this->dataPtr->requestMsg.reset(nullptr); } ///////////////////////////////////////////////// diff --git a/gazebo/rendering/Scene.hh b/gazebo/rendering/Scene.hh index 052dac2b4c..7d4687d037 100644 --- a/gazebo/rendering/Scene.hh +++ b/gazebo/rendering/Scene.hh @@ -661,6 +661,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); diff --git a/gazebo/rendering/ScenePrivate.hh b/gazebo/rendering/ScenePrivate.hh index 6bdd9eed0f..7ed79b5c7b 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; 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) diff --git a/test/integration/shadow_caster_render_back_faces.cc b/test/integration/shadow_caster_render_back_faces.cc index 3c1269e5d5..24ae798f73 100644 --- a/test/integration/shadow_caster_render_back_faces.cc +++ b/test/integration/shadow_caster_render_back_faces.cc @@ -107,7 +107,7 @@ TEST_F(ShadowCasterRenderBackFacesTest, ShadowCasterBackFaces) } ///////////////////////////////////////////////// -// \brief The shadow caster will not render back faces; The plane in the world +// \brief The shadow caster will not render back faces; The plane in the world /// will not cast a shadow void ShadowCasterRenderBackFacesTest::ShadowCasterNoBackFaces() { 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;