From 4ac1c19cfc61c75681c181b79d4790b7712e5de1 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 13:15:57 +0200 Subject: [PATCH] Move private method to helpers file --- .../gazebo/include/scenario/gazebo/helpers.h | 6 ++ cpp/scenario/gazebo/src/GazeboSimulator.cpp | 86 +++---------------- cpp/scenario/gazebo/src/helpers.cpp | 52 +++++++++++ 3 files changed, 68 insertions(+), 76 deletions(-) diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h index 41e8d7208..7c8f720c5 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h @@ -142,6 +142,12 @@ namespace scenario::gazebo::utils { const std::string& newModelName, size_t modelIndex = 0); + bool updateSDFPhysics(sdf::Root& sdfRoot, + const double maxStepSize, + const double rtf, + const double realTimeUpdateRate, + const size_t worldIndex = 0); + sdf::ElementPtr getPluginSDFElement(const std::string& libName, const std::string& className); diff --git a/cpp/scenario/gazebo/src/GazeboSimulator.cpp b/cpp/scenario/gazebo/src/GazeboSimulator.cpp index b005b84e3..e6c6a5618 100644 --- a/cpp/scenario/gazebo/src/GazeboSimulator.cpp +++ b/cpp/scenario/gazebo/src/GazeboSimulator.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -117,9 +116,7 @@ class GazeboSimulator::Impl using WorldName = std::string; std::unordered_map worlds; - bool setPhysics(const detail::PhysicsData& physicsData); detail::PhysicsData getPhysicsData(const size_t worldIndex) const; - bool sceneBroadcasterActive(const std::string& worldName); }; @@ -536,9 +533,16 @@ std::shared_ptr GazeboSimulator::Impl::getServer() // overriding the default profile. // NOTE: this could be avoided if gazebo::Server would expose the // SimulationRunner::SetStepSize method. - if (!this->setPhysics(gazebo.physics)) { - gymppError << "Failed to set physics profile" << std::endl; - return nullptr; + for (size_t worldIdx = 0; worldIdx < sdf->WorldCount(); ++worldIdx) { + if (!utils::updateSDFPhysics(*sdf, + gazebo.physics.maxStepSize, + gazebo.physics.rtf, + /*realTimeUpdateRate=*/-1, + worldIdx)) { + gymppError << "Failed to set physics profile" << std::endl; + return nullptr; + } + assert(this->getPhysicsData(worldIdx) == gazebo.physics); } gymppDebug << "Physics profile:" << std::endl @@ -611,76 +615,6 @@ std::shared_ptr GazeboSimulator::Impl::getServer() return gazebo.server; } -bool GazeboSimulator::Impl::setPhysics(const detail::PhysicsData& physicsData) -{ - if (physicsData.rtf <= 0) { - gymppError << "Invalid RTF value (" << physicsData.rtf << ")" - << std::endl; - return false; - } - - if (physicsData.maxStepSize <= 0) { - gymppError << "Invalid physics step size (" << physicsData.maxStepSize - << ")" << std::endl; - return false; - } - - if (std::isinf(physicsData.maxStepSize)) { - gymppError << "The physics step size cannot be infinite" << std::endl; - return false; - } - - assert(sdf); - for (size_t idx = 0; idx < sdf->WorldCount(); ++idx) { - - const sdf::World* world = sdf->WorldByIndex(idx); - - if (world->PhysicsCount() != 1) { - gymppError << "Found more than one physics profile." << std::endl; - return false; - } - - // Set the physics properties using the helper. - // This sets the internal value but it does not update the DOM. - auto* physics = const_cast(world->PhysicsByIndex(0)); - physics->SetMaxStepSize(physicsData.maxStepSize); - physics->SetRealTimeFactor(physicsData.rtf); - } - - // Update the DOM operating directly on the raw elements - sdf::ElementPtr world = sdf->Element()->GetElement("world"); - - if (!world) { - gymppError << "Failed to find any world" << std::endl; - return false; - } - - while (world) { - sdf::ElementPtr physics = world->GetElement("physics"); - assert(physics); - - sdf::ElementPtr max_step_size = physics->GetElement("max_step_size"); - max_step_size->AddValue( - "double", std::to_string(physicsData.maxStepSize), true); - - sdf::ElementPtr real_time_update_rate = - physics->GetElement("real_time_update_rate"); - real_time_update_rate->AddValue( - "double", std::to_string(physicsData.realTimeUpdateRate), true); - - sdf::ElementPtr real_time_factor = - physics->GetElement("real_time_factor"); - real_time_factor->AddValue( - "double", std::to_string(physicsData.rtf), true); - - world = world->GetNextElement("world"); - } - - assert(this->getPhysicsData(0) == gazebo.physics); - - return true; -} - detail::PhysicsData GazeboSimulator::Impl::getPhysicsData(const size_t worldIndex) const { diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp index 5d41076c3..0401bb7f9 100644 --- a/cpp/scenario/gazebo/src/helpers.cpp +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -279,6 +280,57 @@ bool utils::renameSDFModel(sdf::Root& sdfRoot, return true; } +bool utils::updateSDFPhysics(sdf::Root& sdfRoot, + const double maxStepSize, + const double rtf, + const double realTimeUpdateRate, + const size_t worldIndex) +{ + if (rtf <= 0) { + gymppError << "Invalid RTF value (" << rtf << ")" << std::endl; + return false; + } + + if (maxStepSize <= 0) { + gymppError << "Invalid physics max step size (" << maxStepSize << ")" + << std::endl; + return false; + } + + const sdf::World* world = sdfRoot.WorldByIndex(worldIndex); + + if (world->PhysicsCount() != 1) { + gymppError << "Found more than one physics profile" << std::endl; + return false; + } + + // Set the physics properties using the helper. + // It sets the internal value but it does not update the DOM. + auto* physics = const_cast(world->PhysicsByIndex(0)); + physics->SetMaxStepSize(maxStepSize); + physics->SetRealTimeFactor(rtf); + + // Update the DOM operating directly on the raw elements + sdf::ElementPtr worldElement = world->Element(); + + sdf::ElementPtr physicsElement = worldElement->GetElement("physics"); + assert(physicsElement); + + sdf::ElementPtr max_step_size = physicsElement->GetElement("max_step_size"); + max_step_size->AddValue("double", std::to_string(maxStepSize), true); + + sdf::ElementPtr real_time_update_rate = + physicsElement->GetElement("real_time_update_rate"); + real_time_update_rate->AddValue( + "double", std::to_string(realTimeUpdateRate), true); + + sdf::ElementPtr real_time_factor = + physicsElement->GetElement("real_time_factor"); + real_time_factor->AddValue("double", std::to_string(rtf), true); + + return true; +} + sdf::ElementPtr utils::getPluginSDFElement(const std::string& libName, const std::string& className) {