From 7aad6ccaaefb7f455774c6daa5058bb7de6c201e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Sep 2021 10:26:12 +0200 Subject: [PATCH 1/3] Minor cleanup of GazeboSimulator --- scenario/src/gazebo/src/GazeboSimulator.cpp | 259 ++++++++++---------- 1 file changed, 132 insertions(+), 127 deletions(-) diff --git a/scenario/src/gazebo/src/GazeboSimulator.cpp b/scenario/src/gazebo/src/GazeboSimulator.cpp index 5a5a6f152..790c2e149 100644 --- a/scenario/src/gazebo/src/GazeboSimulator.cpp +++ b/scenario/src/gazebo/src/GazeboSimulator.cpp @@ -653,158 +653,163 @@ bool GazeboSimulator::Impl::insertSDFWorld(const sdf::World& world) std::shared_ptr GazeboSimulator::Impl::getServer() { // Lazy initialization of the server - if (!gazebo.server) { + if (gazebo.server) { + return gazebo.server; + } - if (gazebo.numOfIterations == 0) { - sError << "Non-deterministic mode (iterations=0) is not " - << "currently supported" << std::endl; - return nullptr; - } + // ================= + // Create the server + // ================= - sdf::Root root; + if (gazebo.numOfIterations == 0) { + sError << "Non-deterministic mode (iterations=0) is not " + << "currently supported" << std::endl; + return nullptr; + } - if (!sdfElement) { - sMessage << "Using default empty world" << std::endl; - auto errors = root.LoadSdfString(utils::getEmptyWorld()); - assert(errors.empty()); // TODO - } - else { - auto errors = root.LoadSdfString(sdfElement->ToString("")); - assert(errors.empty()); // TODO - } + sdf::Root root; - if (root.WorldCount() == 0) { - sError << "Failed to find a world in the SDF root" << std::endl; - return nullptr; - } + if (!sdfElement) { + sMessage << "Using default empty world" << std::endl; + auto errors = root.LoadSdfString(utils::getEmptyWorld()); + assert(errors.empty()); // TODO + } + else { + auto errors = root.LoadSdfString(sdfElement->ToString("")); + assert(errors.empty()); // TODO + } - // Check if there are sdf parsing errors - assert(utils::sdfStringValid(root.Element()->Clone()->ToString(""))); + if (root.WorldCount() == 0) { + sError << "Failed to find a world in the SDF root" << std::endl; + return nullptr; + } - if (utils::verboseFromEnvironment()) { - sDebug << "Loading the following SDF file in the gazebo server:" - << std::endl; - std::cout << root.Element()->ToString("") << std::endl; - } + // Check if there are sdf parsing errors + assert(utils::sdfStringValid(root.Element()->Clone()->ToString(""))); - // Set the following environment variable to disable loading the default - // server plugins, which include upstream's Physics. - // https://github.com/ignitionrobotics/ign-gazebo/pull/281 - // TODO: this will not likely work in Windows. - std::string value; - if (!ignition::common::env( - ignition::gazebo::kServerConfigPathEnv, value, true) - && !ignition::common::setenv(ignition::gazebo::kServerConfigPathEnv, - "")) { - sError << "Failed to set " << ignition::gazebo::kServerConfigPathEnv - << std::endl; - return nullptr; - } + if (utils::verboseFromEnvironment()) { + sDebug << "Loading the following SDF file in the gazebo server:" + << std::endl; + std::cout << root.Element()->ToString("") << std::endl; + } + + // Set the following environment variable to disable loading the default + // server plugins, which include upstream's Physics that is not compatible. + // https://github.com/ignitionrobotics/ign-gazebo/pull/281 + // TODO: this will not likely work in Windows. + std::string value; + if (!ignition::common::env( + ignition::gazebo::kServerConfigPathEnv, value, true) + && !ignition::common::setenv(ignition::gazebo::kServerConfigPathEnv, + "")) { + sError << "Failed to set " << ignition::gazebo::kServerConfigPathEnv + << std::endl; + return nullptr; + } - ignition::gazebo::ServerConfig config; - config.SetSeed(0); - config.SetUseLevels(false); - config.SetSdfString(root.Element()->ToString("")); - - // Create the server. - // The worlds are initialized with the physics parameters - // (rtf and physics step) defined in the SDF. They get overridden - // below. - auto server = std::make_shared(config); - assert(server); - - // Add a Configure-only system to get the ECM pointer - for (size_t worldIdx = 0; worldIdx < root.WorldCount(); ++worldIdx) { - - auto provider = std::make_shared(); - if (const auto ok = server->AddSystem(provider, worldIdx); !ok) { - sError << "Failed to insert ECMProvider to world " << worldIdx - << std::endl; - return nullptr; - } + ignition::gazebo::ServerConfig config; + config.SetSeed(0); + config.SetUseLevels(false); + config.SetSdfString(root.Element()->ToString("")); - // Get the ECM and EventManager pointers - detail::SimulationResources resources; - resources.ecm = provider->ecm; - resources.eventMgr = provider->eventMgr; - this->resources[provider->worldName] = resources; - } + // Create the server. + // The worlds are initialized with the physics parameters + // (rtf and physics step) defined in the SDF. + // They get overridden below. + auto server = std::make_shared(config); + assert(server); - sDebug << "Starting the gazebo server" << std::endl; + // Add a Configure-only system to get the ECM pointer + for (size_t worldIdx = 0; worldIdx < root.WorldCount(); ++worldIdx) { - // TODO: is this redundant now? - if (!server->RunOnce(/*paused=*/true)) { - sError << "Failed to initialize the first gazebo server run" + auto provider = std::make_shared(); + if (const auto ok = server->AddSystem(provider, worldIdx); !ok) { + sError << "Failed to insert ECMProvider to world " << worldIdx << std::endl; return nullptr; } - if (!gazebo.physics.valid()) { - sError << "The physics parameters are not valid" << std::endl; - return nullptr; - } + // Get the ECM and EventManager pointers + detail::SimulationResources resources; + resources.ecm = provider->ecm; + resources.eventMgr = provider->eventMgr; + this->resources[provider->worldName] = resources; + } - // Set the Physics parameters. - // Note: all worlds must share the same parameters. - for (const auto& [worldName, resources] : this->resources) { - - // Get the world entity - const auto worldEntity = resources.ecm->EntityByComponents( - ignition::gazebo::components::World(), - ignition::gazebo::components::Name(worldName)); - - // Create a new PhysicsCmd component - auto& physics = utils::getComponentData< - ignition::gazebo::components::PhysicsCmd>(resources.ecm, - worldEntity); - - // Store the physics parameters. - // They are processed the next simulator step. - physics.set_max_step_size(gazebo.physics.maxStepSize); - physics.set_real_time_factor(gazebo.physics.rtf); - physics.set_real_time_update_rate( - gazebo.physics.realTimeUpdateRate); - } + std::this_thread::sleep_for(std::chrono::seconds(3)); - // Step the server to process the physics parameters. - // This call executes SimulationRunner::SetStepSize, updating the - // rate at which all systems are called. - // Note: it processes only the parameters of the first world. - if (!server->RunOnce(/*paused=*/true)) { - sError << "Failed to step the server to configure the physics" - << std::endl; - return nullptr; - } + sDebug << "Starting the gazebo server" << std::endl; - for (size_t worldIdx = 0; worldIdx < root.WorldCount(); ++worldIdx) { - // Get the world name - const auto& worldName = root.WorldByIndex(worldIdx)->Name(); + // TODO: is this redundant now? + if (!server->RunOnce(/*paused=*/true)) { + sError << "Failed to initialize the first gazebo server run" + << std::endl; + return nullptr; + } - sDebug << "Creating and caching World '" << worldName << "'" - << std::endl; + if (!gazebo.physics.valid()) { + sError << "The physics parameters are not valid" << std::endl; + return nullptr; + } - // Create the world object. - // Note: performing this operation is important because the - // World objects are created and cached. During the first - // initialization, the World objects create important - // componentes like Timestamp and SimulatedTime. - const auto& world = - Impl::CreateGazeboWorld(worldName, this->resources[worldName]); - - if (!(world && world->valid())) { - sError << "Failed to create world " << worldName << std::endl; - return nullptr; - } + // Set the Physics parameters. + // Note: all worlds must share the same parameters. + for (const auto& [worldName, resources] : this->resources) { - // Cache the world object - assert(this->worlds.find(worldName) == this->worlds.end()); - this->worlds[worldName] = world; + // Get the world entity + const auto worldEntity = resources.ecm->EntityByComponents( + ignition::gazebo::components::World(), + ignition::gazebo::components::Name(worldName)); + + // Create a new PhysicsCmd component + auto& physics = + utils::getComponentData( + resources.ecm, worldEntity); + + // Store the physics parameters. + // They are processed the next simulator step. + physics.set_max_step_size(gazebo.physics.maxStepSize); + physics.set_real_time_factor(gazebo.physics.rtf); + physics.set_real_time_update_rate(gazebo.physics.realTimeUpdateRate); + } + + // Step the server to process the physics parameters. + // This call executes SimulationRunner::SetStepSize, updating the + // rate at which all systems are called. + // Note: it processes only the parameters of the first world. + if (!server->RunOnce(/*paused=*/true)) { + sError << "Failed to step the server to configure the physics" + << std::endl; + return nullptr; + } + + for (size_t worldIdx = 0; worldIdx < root.WorldCount(); ++worldIdx) { + // Get the world name + const auto& worldName = root.WorldByIndex(worldIdx)->Name(); + + sDebug << "Creating and caching World '" << worldName << "'" + << std::endl; + + // Create the world object. + // Note: performing this operation is important because the + // World objects are created and cached. During the first + // initialization, the World objects create important + // componentes like Timestamp and SimulatedTime. + const auto& world = + Impl::CreateGazeboWorld(worldName, this->resources[worldName]); + + if (!(world && world->valid())) { + sError << "Failed to create world " << worldName << std::endl; + return nullptr; } - // Store the server - gazebo.server = server; + // Cache the world object + assert(this->worlds.find(worldName) == this->worlds.end()); + this->worlds[worldName] = world; } + // Store and return the server + gazebo.server = server; return gazebo.server; } From f1864af099d539149b85fad9221fdd0d177def06 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Sep 2021 10:27:10 +0200 Subject: [PATCH 2/3] Use upstream method to check canonical link --- scenario/src/gazebo/src/Link.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/scenario/src/gazebo/src/Link.cpp b/scenario/src/gazebo/src/Link.cpp index 0ad9bd850..3871e0e8f 100644 --- a/scenario/src/gazebo/src/Link.cpp +++ b/scenario/src/gazebo/src/Link.cpp @@ -63,16 +63,10 @@ class Link::Impl public: ignition::gazebo::Link link; - static bool IsCanonical(const Link& link) - { - return link.ecm()->EntityHasComponentType( - link.entity(), ignition::gazebo::components::CanonicalLink::typeId); - } - static ignition::math::Pose3d GetWorldPose(const Link& link, const Link::Impl& impl) { - if (!Impl::IsCanonical(link)) { + if (!impl.link.IsCanonical(*link.ecm())) { const auto& linkPoseOptional = impl.link.WorldPose(*link.ecm()); if (!linkPoseOptional.has_value()) { From 17fafe5daee6414d3f7e5984ff4183324dca36fe Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Sep 2021 20:41:11 +0200 Subject: [PATCH 3/3] Get data by const reference --- scenario/src/gazebo/src/Link.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scenario/src/gazebo/src/Link.cpp b/scenario/src/gazebo/src/Link.cpp index 3871e0e8f..0d0cb9f63 100644 --- a/scenario/src/gazebo/src/Link.cpp +++ b/scenario/src/gazebo/src/Link.cpp @@ -531,7 +531,7 @@ bool Link::applyWorldWrench(const std::array& force, assert(entityWithSimTime != ignition::gazebo::kNullEntity); // Get the current simulated time - auto& now = utils::getExistingComponentData< + const auto& now = utils::getExistingComponentData< ignition::gazebo::components::SimulatedTime>(m_ecm, entityWithSimTime); // Create a new wrench with duration