Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Minor cleanup #395

Merged
merged 3 commits into from
Sep 29, 2021
Merged

Minor cleanup #395

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7aad6cc
Minor cleanup of GazeboSimulator
diegoferigo Sep 15, 2021
f1864af
Use upstream method to check canonical link
diegoferigo Sep 15, 2021
17fafe5
Get data by const reference
diegoferigo Sep 29, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
259 changes: 132 additions & 127 deletions scenario/src/gazebo/src/GazeboSimulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -653,158 +653,163 @@ bool GazeboSimulator::Impl::insertSDFWorld(const sdf::World& world)
std::shared_ptr<ignition::gazebo::Server> 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<ignition::gazebo::Server>(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<detail::ECMProvider>();
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<ignition::gazebo::Server>(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<detail::ECMProvider>();
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<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);
}

// 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;
}

Expand Down
10 changes: 2 additions & 8 deletions scenario/src/gazebo/src/Link.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down Expand Up @@ -537,7 +531,7 @@ bool Link::applyWorldWrench(const std::array<double, 3>& 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
Expand Down