From 7713502f42169eeeaba1c7fe54e6c81aec6fd216 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 30 Apr 2020 23:08:37 +0200 Subject: [PATCH 1/2] Style gympp edits --- cpp/gympp/base/Space.cpp | 27 ++++++++++++------- cpp/gympp/base/include/gympp/base/Common.h | 11 +++++--- .../base/include/gympp/base/Environment.h | 3 --- cpp/gympp/base/include/gympp/base/Task.h | 2 +- 4 files changed, 27 insertions(+), 16 deletions(-) diff --git a/cpp/gympp/base/Space.cpp b/cpp/gympp/base/Space.cpp index 8d8cb0373..263df0fd9 100644 --- a/cpp/gympp/base/Space.cpp +++ b/cpp/gympp/base/Space.cpp @@ -85,7 +85,9 @@ bool Box::contains(const Space::Sample& data) const // Check the type if (!bufferPtr) { - gymppError << "Failed to get the buffer or the supported type from the sample" << std::endl; + gymppError + << "Failed to get the buffer or the supported type from the sample" + << std::endl; return false; } @@ -94,14 +96,17 @@ bool Box::contains(const Space::Sample& data) const if (bufferPtr->size() != pImpl->shape[0]) { gymppError << "The size of the buffer (" << bufferPtr->size() - << ") does not match with the shape of the space (" << pImpl->shape[0] << ")" - << std::endl; + << ") does not match with the shape of the space (" + << pImpl->shape[0] << ")" << std::endl; return false; } for (unsigned i = 0; i < bufferPtr->size(); ++i) { - if (((*bufferPtr)[i] > pImpl->high[i]) || ((*bufferPtr)[i] < pImpl->low[i])) { - gymppError << "The sample does not comply to the limits set for its space" << std::endl; + if (((*bufferPtr)[i] > pImpl->high[i]) + || ((*bufferPtr)[i] < pImpl->low[i])) { + gymppError + << "The sample does not comply to the limits set for its space" + << std::endl; return false; } } @@ -165,7 +170,9 @@ bool Discrete::contains(const Space::Sample& data) const // Check the type if (!bufferPtr) { - gymppError << "Failed to get the buffer or the supported type from the sample" << std::endl; + gymppError + << "Failed to get the buffer or the supported type from the sample" + << std::endl; return false; } @@ -173,13 +180,15 @@ bool Discrete::contains(const Space::Sample& data) const assert(pImpl->shape.size() == 1); if (bufferPtr->size() != pImpl->shape.size()) { gymppError << "The size of the buffer (" << bufferPtr->size() - << ") does not match with the shape of the space (" << pImpl->shape.size() << ")" - << std::endl; + << ") does not match with the shape of the space (" + << pImpl->shape.size() << ")" << std::endl; return false; } if ((*bufferPtr)[0] < 0 || (*bufferPtr)[0] >= pImpl->n) { - gymppError << "The sample does not comply to the limits set for its space" << std::endl; + gymppError + << "The sample does not comply to the limits set for its space" + << std::endl; return false; } diff --git a/cpp/gympp/base/include/gympp/base/Common.h b/cpp/gympp/base/include/gympp/base/Common.h index db9e622bb..1413e75f1 100644 --- a/cpp/gympp/base/include/gympp/base/Common.h +++ b/cpp/gympp/base/include/gympp/base/Common.h @@ -31,7 +31,8 @@ namespace gympp { using BufferLong = BufferContainer::type; using BufferFloat = BufferContainer::type; using BufferDouble = BufferContainer::type; - using GenericBuffer = std::variant; + using GenericBuffer = + std::variant; namespace data { using Shape = std::vector; @@ -63,7 +64,8 @@ struct gympp::base::data::Sample template std::optional get(const size_t i) const { - auto bufferPtr = std::get_if::type>(&buffer); + auto bufferPtr = + std::get_if::type>(&buffer); if (!bufferPtr) { return {}; @@ -100,7 +102,10 @@ struct gympp::base::Range DataSupport min = 0; DataSupport max = 0; - bool contains(double value) { return (value <= max && value >= min) ? true : false; } + bool contains(double value) + { + return (value <= max && value >= min) ? true : false; + } }; #endif // GYMPP_BASE_COMMON diff --git a/cpp/gympp/base/include/gympp/base/Environment.h b/cpp/gympp/base/include/gympp/base/Environment.h index 17b230341..459fb03c1 100644 --- a/cpp/gympp/base/include/gympp/base/Environment.h +++ b/cpp/gympp/base/include/gympp/base/Environment.h @@ -38,9 +38,6 @@ struct gympp::base::State gympp::base::Observation observation; }; -// TODO: https://hub.packtpub.com/openai-gym-environments-wrappers-and-monitors-tutorial/ -// These C++ and their mapping to python / julia should allow using the Wrapper method - // From https://github.com/openai/gym/blob/master/gym/core.py class gympp::base::Environment { diff --git a/cpp/gympp/base/include/gympp/base/Task.h b/cpp/gympp/base/include/gympp/base/Task.h index cedc11a8b..f5df1bc81 100644 --- a/cpp/gympp/base/include/gympp/base/Task.h +++ b/cpp/gympp/base/include/gympp/base/Task.h @@ -15,7 +15,7 @@ namespace gympp { namespace base { class Task; - } // namespace gazebo + } // namespace base } // namespace gympp class gympp::base::Task From 6bc2496806bde89846db8b95f2131b8cfce6fa29 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 30 Apr 2020 23:16:20 +0200 Subject: [PATCH 2/2] Rename log macros from gympp* to s* --- .../src/ComputedTorqueFixedBase.cpp | 30 ++--- .../gazebo/include/scenario/gazebo/Joint.h | 7 +- .../gazebo/include/scenario/gazebo/Log.h | 8 +- cpp/scenario/gazebo/src/GazeboSimulator.cpp | 107 ++++++++--------- cpp/scenario/gazebo/src/Joint.cpp | 113 +++++++++--------- cpp/scenario/gazebo/src/Link.cpp | 9 +- cpp/scenario/gazebo/src/Model.cpp | 40 +++---- cpp/scenario/gazebo/src/World.cpp | 28 ++--- cpp/scenario/gazebo/src/helpers.cpp | 37 +++--- cpp/scenario/gazebo/src/signals.cpp | 11 +- cpp/scenario/gazebo/src/utils.cpp | 40 +++---- .../ControllerRunner/ControllerRunner.cpp | 55 ++++----- .../ControllerRunner/ControllersFactory.cpp | 29 ++--- .../plugins/ECMProvider/ECMProvider.cpp | 18 +-- .../plugins/ECMProvider/ECMSingleton.cpp | 32 ++--- .../JointController/JointController.cpp | 24 ++-- 16 files changed, 285 insertions(+), 303 deletions(-) diff --git a/cpp/scenario/controllers/src/ComputedTorqueFixedBase.cpp b/cpp/scenario/controllers/src/ComputedTorqueFixedBase.cpp index 4655e818b..e27f12ff7 100644 --- a/cpp/scenario/controllers/src/ComputedTorqueFixedBase.cpp +++ b/cpp/scenario/controllers/src/ComputedTorqueFixedBase.cpp @@ -136,29 +136,29 @@ ComputedTorqueFixedBase::~ComputedTorqueFixedBase() {} bool ComputedTorqueFixedBase::initialize() { - gymppDebug << "Initializing ComputedTorqueFixedBaseCpp" << std::endl; + sDebug << "Initializing ComputedTorqueFixedBaseCpp" << std::endl; if (pImpl->kinDyn) { - gymppWarning + sWarning << "The KinDynComputations object has been already initialized" << std::endl; return true; } if (!(m_model && m_model->valid())) { - gymppError << "Couldn't initialize controller. Model not valid." + sError << "Couldn't initialize controller. Model not valid." << std::endl; return false; } if (m_controlledJoints.size() != m_model->dofs() || m_controlledJoints.empty()) { - gymppError << "The list of controlled joints is not valid" << std::endl; + sError << "The list of controlled joints is not valid" << std::endl; return false; } if (m_controlledJoints.empty()) { - gymppDebug << "No list of controlled joints. Controlling all the " + sDebug << "No list of controlled joints. Controlling all the " "robots joints." << std::endl; @@ -170,7 +170,7 @@ bool ComputedTorqueFixedBase::initialize() for (auto& joint : m_model->joints(m_controlledJoints)) { if (joint->dofs() != 1) { - gymppError << "Joint '" << joint->name() + sError << "Joint '" << joint->name() << "' does not have 1 DoF and is not supported" << std::endl; return false; @@ -179,7 +179,7 @@ bool ComputedTorqueFixedBase::initialize() iDynTree::ModelLoader loader; if (!loader.loadReducedModelFromFile(pImpl->urdfFile, m_controlledJoints)) { - gymppError << "Failed to load reduced model from the urdf file" + sError << "Failed to load reduced model from the urdf file" << std::endl; return false; } @@ -189,7 +189,7 @@ bool ComputedTorqueFixedBase::initialize() iDynTree::MIXED_REPRESENTATION); if (!pImpl->kinDyn->loadRobotModel(loader.model())) { - gymppError << "Failed to insert model in the KinDynComputations object" + sError << "Failed to insert model in the KinDynComputations object" << std::endl; return false; } @@ -199,14 +199,14 @@ bool ComputedTorqueFixedBase::initialize() pImpl->initialValues.controlMode[joint->name()] = joint->controlMode(); if (!joint->setControlMode(base::JointControlMode::Force)) { - gymppError << "Failed to control joint '" << joint->name() + sError << "Failed to control joint '" << joint->name() << "' in Force" << std::endl; return false; } } // Initialize buffers - gymppDebug << "Controlling " << m_controlledJoints.size() << " DoFs" + sDebug << "Controlling " << m_controlledJoints.size() << " DoFs" << std::endl; pImpl->buffers = std::make_unique(m_controlledJoints.size()); @@ -283,7 +283,7 @@ bool ComputedTorqueFixedBase::step(const Controller::StepSize& /*dt*/) if (!m_model->setJointGeneralizedForceTargets(pImpl->buffers->torquesVector, m_controlledJoints)) { - gymppError << "Failed to set joint forces" << std::endl; + sError << "Failed to set joint forces" << std::endl; return false; } @@ -300,7 +300,7 @@ bool ComputedTorqueFixedBase::terminate() auto joint = m_model->getJoint(jointName); if (!joint->setControlMode(controlMode)) { - gymppError << "Failed to restore original control mode of joint '" + sError << "Failed to restore original control mode of joint '" << jointName << "'" << std::endl; ok = ok && false; } @@ -332,17 +332,17 @@ bool ComputedTorqueFixedBase::updateStateFromModel() if (!pImpl->kinDyn->setRobotState(pImpl->buffers->jointPositions, pImpl->buffers->jointVelocities, pImpl->buffers->gravity)) { - gymppError << "Failed to set the robot state" << std::endl; + sError << "Failed to set the robot state" << std::endl; return false; } if (!pImpl->kinDyn->getFreeFloatingMassMatrix(pImpl->buffers->massMatrix)) { - gymppError << "Failed to get the mass matrix" << std::endl; + sError << "Failed to get the mass matrix" << std::endl; return false; } if (!pImpl->kinDyn->generalizedBiasForces(pImpl->buffers->biasForces)) { - gymppError << "Failed to get the bias forces " << std::endl; + sError << "Failed to get the bias forces " << std::endl; return false; } diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h index 63416ced7..7b1724f21 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h @@ -211,10 +211,9 @@ struct scenario::base::JointLimit : JointLimit(std::min(_min.size(), _max.size())) { if (_min.size() != _max.size()) { - gymppWarning - << "The max and min limits have different size. " - << "Ignoring the limits and using the smaller dimension." - << std::endl; + sWarning << "The max and min limits have different size. " + << "Ignoring the limits and using the smaller dimension." + << std::endl; return; } diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Log.h b/cpp/scenario/gazebo/include/scenario/gazebo/Log.h index 4e0b52189..fd1b3f2f0 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Log.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Log.h @@ -28,10 +28,10 @@ #define SCENARIO_GAZEBO_LOG #include -#define gymppError ::ignition::common::Console::err(__FILE__, __LINE__) -#define gymppWarning ::ignition::common::Console::warn(__FILE__, __LINE__) -#define gymppMessage ::ignition::common::Console::msg(__FILE__, __LINE__) -#define gymppDebug ::ignition::common::Console::dbg(__FILE__, __LINE__) +#define sError ::ignition::common::Console::err(__FILE__, __LINE__) +#define sWarning ::ignition::common::Console::warn(__FILE__, __LINE__) +#define sMessage ::ignition::common::Console::msg(__FILE__, __LINE__) +#define sDebug ::ignition::common::Console::dbg(__FILE__, __LINE__) #define gymppLog ::ignition::common::Console::log(__FILE__, __LINE__) #endif // SCENARIO_GAZEBO_LOG diff --git a/cpp/scenario/gazebo/src/GazeboSimulator.cpp b/cpp/scenario/gazebo/src/GazeboSimulator.cpp index 86fcddad5..9c9fdbd4c 100644 --- a/cpp/scenario/gazebo/src/GazeboSimulator.cpp +++ b/cpp/scenario/gazebo/src/GazeboSimulator.cpp @@ -163,13 +163,13 @@ size_t GazeboSimulator::stepsPerRun() const bool GazeboSimulator::initialize() { if (this->initialized()) { - gymppMessage << "The simulator is already initialized" << std::endl; + sMessage << "The simulator is already initialized" << std::endl; return true; } // Initialize the server if (!pImpl->getServer()) { - gymppError << "Failed to get the Gazebo server" << std::endl; + sError << "Failed to get the Gazebo server" << std::endl; return false; } @@ -196,14 +196,14 @@ bool GazeboSimulator::initialized() const bool GazeboSimulator::run(const bool paused) { if (!this->initialized()) { - gymppError << "The simulator was not initialized" << std::endl; + sError << "The simulator was not initialized" << std::endl; return false; } // Get the gazebo server auto server = pImpl->getServer(); if (!server) { - gymppError << "Failed to get the ignition server" << std::endl; + sError << "Failed to get the ignition server" << std::endl; return false; } @@ -214,8 +214,7 @@ bool GazeboSimulator::run(const bool paused) bool deterministic = pImpl->gazebo.numOfIterations != 0 ? true : false; if (!deterministic && server->Running()) { - gymppWarning << "The server is already running in background" - << std::endl; + sWarning << "The server is already running in background" << std::endl; return true; } @@ -232,7 +231,7 @@ bool GazeboSimulator::run(const bool paused) if (!server->Run(/*blocking=*/deterministic, /*iterations=*/iterations, /*paused=*/paused)) { - gymppError << "The server couldn't execute the step" << std::endl; + sError << "The server couldn't execute the step" << std::endl; return false; } @@ -242,7 +241,7 @@ bool GazeboSimulator::run(const bool paused) bool GazeboSimulator::gui(const int verbosity) { if (!this->initialized()) { - gymppError << "The simulator was not initialized" << std::endl; + sError << "The simulator was not initialized" << std::endl; return false; } @@ -256,7 +255,7 @@ bool GazeboSimulator::gui(const int verbosity) std::vector worldNames = this->worldNames(); if (worldNames.empty()) { - gymppError << "Failed to find any world in the simulator" << std::endl; + sError << "Failed to find any world in the simulator" << std::endl; return false; } @@ -264,12 +263,12 @@ bool GazeboSimulator::gui(const int verbosity) std::string worldName = worldNames[0]; if (!pImpl->sceneBroadcasterActive(worldName)) { - gymppDebug << "Starting the SceneBroadcaster plugin" << std::endl; + sDebug << "Starting the SceneBroadcaster plugin" << std::endl; auto world = this->getWorld(worldName); if (!world->insertWorldPlugin( "libignition-gazebo-scene-broadcaster-system.so", "ignition::gazebo::systems::SceneBroadcaster")) { - gymppError << "Failed to load SceneBroadcaster plugin" << std::endl; + sError << "Failed to load SceneBroadcaster plugin" << std::endl; } } @@ -290,7 +289,7 @@ bool GazeboSimulator::gui(const int verbosity) std::vector serviceList; do { - gymppDebug << "Waiting GUI to show up... " << std::endl; + sDebug << "Waiting GUI to show up... " << std::endl; node.ServiceList(serviceList); for (const auto& serviceName : serviceList) { @@ -303,7 +302,7 @@ bool GazeboSimulator::gui(const int verbosity) std::this_thread::sleep_for(std::chrono::milliseconds(50)); } while (!guiServiceExists); - gymppDebug << "GUI up and running" << std::endl; + sDebug << "GUI up and running" << std::endl; return true; } @@ -333,8 +332,8 @@ bool GazeboSimulator::close() // This happens while tearing down everything. The ECMProvider plugin // sometimes is destroyed before the simulator. catch (std::runtime_error) { - gymppWarning << "Failed to clean the singleton from the worlds" - << std::endl; + sWarning << "Failed to clean the singleton from the worlds" + << std::endl; } } @@ -347,13 +346,13 @@ bool GazeboSimulator::close() bool GazeboSimulator::pause() { if (!this->initialized()) { - gymppMessage << "Couldn't pause the simulator, it was never initialized" - << std::endl; + sMessage << "Couldn't pause the simulator, it was never initialized" + << std::endl; return true; } if (!this->running()) { - gymppMessage << "The simulation is already paused" << std::endl; + sMessage << "The simulation is already paused" << std::endl; return true; } @@ -369,7 +368,7 @@ bool GazeboSimulator::pause() bool GazeboSimulator::running() const { if (!this->initialized()) { - gymppMessage << "The simulator was not initialized" << std::endl; + sMessage << "The simulator was not initialized" << std::endl; return false; } @@ -386,8 +385,8 @@ bool GazeboSimulator::insertWorldFromSDF(const std::string& worldFile, sdfRoot = utils::getSdfRootFromFile(worldFile); } else { - gymppMessage << "No world file passed, using the default empty world" - << std::endl; + sMessage << "No world file passed, using the default empty world" + << std::endl; sdfRoot = utils::getSdfRootFromString(utils::getEmptyWorld()); } @@ -397,7 +396,7 @@ bool GazeboSimulator::insertWorldFromSDF(const std::string& worldFile, } if (sdfRoot->WorldCount() != 1) { - gymppError << "The world file has more than one world" << std::endl; + sError << "The world file has more than one world" << std::endl; return false; } @@ -428,13 +427,13 @@ bool GazeboSimulator::insertWorldsFromSDF( } if (sdfRoot->WorldCount() == 0) { - gymppError << "Failed to find any world in the SDF file" << std::endl; + sError << "Failed to find any world in the SDF file" << std::endl; return false; } if (!worldNames.empty() && sdfRoot->WorldCount() != worldNames.size()) { - gymppError << "The number of world names does not match the number of " - << "worlds found in the SDF file" << std::endl; + sError << "The number of world names does not match the number of " + << "worlds found in the SDF file" << std::endl; return false; } @@ -455,8 +454,8 @@ bool GazeboSimulator::insertWorldsFromSDF( } if (!pImpl->insertWorld(thisWorld)) { - gymppError << "Failed to insert world " << thisWorld.Name() - << std::endl; + sError << "Failed to insert world " << thisWorld.Name() + << std::endl; return false; } } @@ -467,7 +466,7 @@ bool GazeboSimulator::insertWorldsFromSDF( std::vector GazeboSimulator::worldNames() const { if (!this->initialized()) { - gymppError << "The simulator was not initialized" << std::endl; + sError << "The simulator was not initialized" << std::endl; return {}; } @@ -482,7 +481,7 @@ std::shared_ptr GazeboSimulator::getWorld(const std::string& worldName) const { if (!this->initialized()) { - gymppError << "The simulator was not initialized" << std::endl; + sError << "The simulator was not initialized" << std::endl; return nullptr; } @@ -499,8 +498,8 @@ GazeboSimulator::getWorld(const std::string& worldName) const returnedWorldName = worldNames[0]; } else { - gymppError << "Found multiple worlds. " - << "You must specify the world name." << std::endl; + sError << "Found multiple worlds. " + << "You must specify the world name." << std::endl; return nullptr; } } @@ -513,21 +512,21 @@ GazeboSimulator::getWorld(const std::string& worldName) const if (std::find(worldNames.begin(), worldNames.end(), returnedWorldName) == worldNames.end()) { - gymppError << "Failed to find world '" << returnedWorldName << "'" - << std::endl; + sError << "Failed to find world '" << returnedWorldName << "'" + << std::endl; return nullptr; } auto& ecmSingleton = scenario::plugins::gazebo::ECMSingleton::Instance(); if (!ecmSingleton.hasWorld(returnedWorldName)) { - gymppError << "Failed to find world in the singleton" << std::endl; + sError << "Failed to find world in the singleton" << std::endl; return nullptr; } if (!ecmSingleton.valid(returnedWorldName)) { - gymppError << "Resources of world " << worldName << " not valid" - << std::endl; + sError << "Resources of world " << worldName << " not valid" + << std::endl; return nullptr; } @@ -569,8 +568,8 @@ bool GazeboSimulator::Impl::insertWorld(const sdf::World& world) } if (root->WorldNameExists(world.Name())) { - gymppError << "Another world with name " << world.Name() - << " already exists" << std::endl; + sError << "Another world with name " << world.Name() + << " already exists" << std::endl; return false; } @@ -587,15 +586,15 @@ std::shared_ptr GazeboSimulator::Impl::getServer() if (!gazebo.server) { if (gazebo.numOfIterations == 0) { - gymppError << "Non-deterministic mode (iterations=0) is not " - << "currently supported" << std::endl; + sError << "Non-deterministic mode (iterations=0) is not " + << "currently supported" << std::endl; return nullptr; } sdf::Root root; if (!sdfElement) { - gymppMessage << "Using default empty world" << std::endl; + sMessage << "Using default empty world" << std::endl; auto errors = root.LoadSdfString(utils::getEmptyWorld()); assert(errors.empty()); // TODO } @@ -605,7 +604,7 @@ std::shared_ptr GazeboSimulator::Impl::getServer() } if (root.WorldCount() == 0) { - gymppError << "Failed to find a world in the SDF root" << std::endl; + sError << "Failed to find a world in the SDF root" << std::endl; return nullptr; } @@ -634,19 +633,19 @@ std::shared_ptr GazeboSimulator::Impl::getServer() gazebo.physics.rtf, /*realTimeUpdateRate=*/-1, worldIdx)) { - gymppError << "Failed to set physics profile" << std::endl; + sError << "Failed to set physics profile" << std::endl; return nullptr; } assert(Impl::getPhysicsData(root, worldIdx) == gazebo.physics); } - gymppDebug << "Physics profile:" << std::endl - << this->gazebo.physics << std::endl; + sDebug << "Physics profile:" << std::endl + << this->gazebo.physics << std::endl; if (utils::verboseFromEnvironment()) { - gymppDebug << "Loading the following SDF file in the gazebo server:" - << std::endl; + sDebug << "Loading the following SDF file in the gazebo server:" + << std::endl; std::cout << root.Element()->ToString("") << std::endl; } @@ -666,11 +665,11 @@ std::shared_ptr GazeboSimulator::Impl::getServer() auto server = std::make_shared(config); assert(server); - gymppDebug << "Starting the gazebo server" << std::endl; + sDebug << "Starting the gazebo server" << std::endl; if (!server->Run( /*blocking=*/true, /*iterations=*/1, /*paused=*/true)) { - gymppError << "Failed to initialize the first gazebo server run" - << std::endl; + sError << "Failed to initialize the first gazebo server run" + << std::endl; return nullptr; } @@ -680,8 +679,8 @@ std::shared_ptr GazeboSimulator::Impl::getServer() // Post-process the world if (!this->postProcessWorld(worldName)) { - gymppError << "Failed to post-process world " << worldName - << std::endl; + sError << "Failed to post-process world " << worldName + << std::endl; return nullptr; } } @@ -698,7 +697,7 @@ bool GazeboSimulator::Impl::postProcessWorld(const std::string& worldName) auto& ecmSingeton = plugins::gazebo::ECMSingleton::Instance(); if (!ecmSingeton.hasWorld(worldName)) { - gymppError << "Failed to initialize ECMProvider" << std::endl; + sError << "Failed to initialize ECMProvider" << std::endl; return false; } @@ -711,7 +710,7 @@ bool GazeboSimulator::Impl::postProcessWorld(const std::string& worldName) ignition::gazebo::components::Name(worldName)); if (worldEntity == ignition::gazebo::kNullEntity) { - gymppError << "Couldn't find world entity" << std::endl; + sError << "Couldn't find world entity" << std::endl; return false; } diff --git a/cpp/scenario/gazebo/src/Joint.cpp b/cpp/scenario/gazebo/src/Joint.cpp index be0a22618..b57eaf3d2 100644 --- a/cpp/scenario/gazebo/src/Joint.cpp +++ b/cpp/scenario/gazebo/src/Joint.cpp @@ -99,7 +99,7 @@ bool Joint::initialize(const ignition::gazebo::Entity jointEntity, ignition::gazebo::EventManager* eventManager) { if (jointEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { - gymppError << "Failed to initialize Joint" << std::endl; + sError << "Failed to initialize Joint" << std::endl; return false; } @@ -108,8 +108,8 @@ bool Joint::initialize(const ignition::gazebo::Entity jointEntity, pImpl->eventManager = eventManager; if (this->dofs() > 1) { - gymppError << "Joints with DoFs > 1 are not currently supported" - << std::endl; + sError << "Joints with DoFs > 1 are not currently supported" + << std::endl; return false; } @@ -118,8 +118,8 @@ bool Joint::initialize(const ignition::gazebo::Entity jointEntity, bool Joint::createECMResources() { - gymppMessage << " [" << pImpl->jointEntity << "] " << this->name() - << std::endl; + sMessage << " [" << pImpl->jointEntity << "] " << this->name() + << std::endl; using namespace ignition::gazebo; @@ -239,7 +239,7 @@ scenario::base::JointControlMode Joint::controlMode() const bool Joint::setControlMode(const scenario::base::JointControlMode mode) { if (mode == base::JointControlMode::PositionInterpolated) { - gymppError << "PositionInterpolated not yet supported" << std::endl; + sError << "PositionInterpolated not yet supported" << std::endl; return false; } @@ -248,8 +248,8 @@ bool Joint::setControlMode(const scenario::base::JointControlMode mode) pImpl->ecm, pImpl->jointEntity, mode); // Delete the existing targets if they exist - gymppDebug << "Deleting existing position and velocity targets after " - << "changing control mode" << std::endl; + sDebug << "Deleting existing position and velocity targets after " + << "changing control mode" << std::endl; pImpl->ecm->RemoveComponent( pImpl->jointEntity, ignition::gazebo::components::JointPositionTarget().TypeId()); @@ -323,8 +323,8 @@ bool Joint::setPID(const scenario::base::PID& pid) }; if (this->dofs() > 1) { - gymppError << "Setting PIDs of joints with more than 1 DoF is not " - << "currently supported" << std::endl; + sError << "Setting PIDs of joints with more than 1 DoF is not " + << "currently supported" << std::endl; return false; } @@ -333,9 +333,9 @@ bool Joint::setPID(const scenario::base::PID& pid) auto maxForce = this->maxGeneralizedForce(0); if (pidParams.cmdMin < minForce || pidParams.cmdMax > maxForce) { - gymppWarning << "The output limits of the PID are less limiting than " - << "the maximum force that can be exerted on the joint. " - << "Ignoring the specified PID limits." << std::endl; + sWarning << "The output limits of the PID are less limiting than " + << "the maximum force that can be exerted on the joint. " + << "Ignoring the specified PID limits." << std::endl; pidParams.cmdMin = minForce; pidParams.cmdMax = maxForce; } @@ -354,8 +354,8 @@ bool Joint::setPID(const scenario::base::PID& pid) scenario::base::Limit Joint::positionLimit(const size_t dof) const { if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return base::Limit(0, 0); } @@ -375,8 +375,8 @@ double Joint::maxGeneralizedForce(const size_t dof) const bool Joint::setMaxGeneralizedForce(const double maxForce, const size_t dof) { if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -418,15 +418,15 @@ bool Joint::setPositionTarget(const double position, const size_t dof) this->controlMode()); if (it == allowedControlModes.end()) { - gymppError + sError << "The active joint control mode does not accept a position target" << std::endl; return false; } if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -448,15 +448,15 @@ bool Joint::setVelocityTarget(const double velocity, const size_t dof) if (!(this->controlMode() == base::JointControlMode::Velocity || this->controlMode() == base::JointControlMode::Idle || this->controlMode() == base::JointControlMode::Force)) { - gymppError + sError << "The active joint control mode does not accept a velocity target" << std::endl; return false; } if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -477,15 +477,15 @@ bool Joint::setAccelerationTarget(const double acceleration, const size_t dof) { if (!(this->controlMode() == base::JointControlMode::Idle || this->controlMode() == base::JointControlMode::Force)) { - gymppError + sError << "The active joint control mode does not accept a velocity target" << std::endl; return false; } if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -515,15 +515,14 @@ bool Joint::setGeneralizedForceTarget(const double force, const size_t dof) this->controlMode()); if (it == allowedControlModes.end()) { - gymppError - << "The active joint control mode does not accept a force target" - << std::endl; + sError << "The active joint control mode does not accept a force target" + << std::endl; return false; } if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -573,8 +572,8 @@ double Joint::generalizedForceTarget(const size_t dof) const bool Joint::resetPosition(const double position, size_t dof) { if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; return false; } @@ -594,8 +593,8 @@ bool Joint::resetPosition(const double position, size_t dof) bool Joint::resetVelocity(const double velocity, const size_t dof) { if (dof >= this->dofs()) { - gymppError << "Joint '" << this->name() << " does not have DoF#" << dof - << std::endl; + sError << "Joint '" << this->name() << " does not have DoF#" << dof + << std::endl; return false; } @@ -620,8 +619,8 @@ bool Joint::reset(const double position, const double velocity, size_t dof) ok = ok && this->resetVelocity(velocity, dof); if (!ok) { - gymppError << "Failed to reset state of joint '" << this->name() << "'" - << std::endl; + sError << "Failed to reset state of joint '" << this->name() << "'" + << std::endl; return false; } @@ -648,14 +647,13 @@ scenario::base::JointLimit Joint::jointPositionLimit() const break; } case base::JointType::Fixed: - gymppWarning - << "Fixed joints do not have DOFs, limits are not defined" - << std::endl; + sWarning << "Fixed joints do not have DOFs, limits are not defined" + << std::endl; break; case base::JointType::Invalid: case base::JointType::Ball: - gymppWarning << "Type of Joint '" << this->name() - << "' has no limits" << std::endl; + sWarning << "Type of Joint '" << this->name() << "' has no limits" + << std::endl; break; } @@ -674,8 +672,8 @@ std::vector Joint::jointMaxGeneralizedForce() const bool Joint::setJointMaxGeneralizedForce(const std::vector& maxForce) { if (maxForce.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -718,8 +716,8 @@ std::vector Joint::jointVelocity() const bool Joint::setJointPositionTarget(const std::vector& position) { if (position.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -734,8 +732,8 @@ bool Joint::setJointPositionTarget(const std::vector& position) bool Joint::setJointVelocityTarget(const std::vector& velocity) { if (velocity.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -750,8 +748,8 @@ bool Joint::setJointVelocityTarget(const std::vector& velocity) bool Joint::setJointAccelerationTarget(const std::vector& acceleration) { if (acceleration.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -766,8 +764,8 @@ bool Joint::setJointAccelerationTarget(const std::vector& acceleration) bool Joint::setJointGeneralizedForceTarget(const std::vector& force) { if (force.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -846,8 +844,8 @@ std::vector Joint::jointGeneralizedForceTarget() const bool Joint::resetJointPosition(const std::vector& position) { if (position.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -871,8 +869,8 @@ bool Joint::resetJointPosition(const std::vector& position) bool Joint::resetJointVelocity(const std::vector& velocity) { if (velocity.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; + sError << "Wrong number of elements (joint_dofs=" << this->dofs() << ")" + << std::endl; return false; } @@ -902,8 +900,7 @@ bool Joint::resetJoint(const std::vector& position, ok = ok && this->resetJointVelocity(velocity); if (!ok) { - gymppError << "Failed to reset joint '" << this->name() << "'" - << std::endl; + sError << "Failed to reset joint '" << this->name() << "'" << std::endl; return false; } diff --git a/cpp/scenario/gazebo/src/Link.cpp b/cpp/scenario/gazebo/src/Link.cpp index dd60fe83a..c2ef90eac 100644 --- a/cpp/scenario/gazebo/src/Link.cpp +++ b/cpp/scenario/gazebo/src/Link.cpp @@ -104,7 +104,7 @@ bool Link::initialize(const ignition::gazebo::Entity linkEntity, ignition::gazebo::EventManager* eventManager) { if (linkEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { - gymppError << "Failed to initialize Link" << std::endl; + sError << "Failed to initialize Link" << std::endl; return false; } @@ -116,7 +116,7 @@ bool Link::initialize(const ignition::gazebo::Entity linkEntity, // Check that the link is valid if (!pImpl->link.Valid(*ecm)) { - gymppError << "The link entity is not valid" << std::endl; + sError << "The link entity is not valid" << std::endl; return false; } @@ -125,8 +125,7 @@ bool Link::initialize(const ignition::gazebo::Entity linkEntity, bool Link::createECMResources() { - gymppMessage << " [" << pImpl->linkEntity << "] " << this->name() - << std::endl; + sMessage << " [" << pImpl->linkEntity << "] " << this->name() << std::endl; using namespace ignition::gazebo; @@ -151,7 +150,7 @@ bool Link::createECMResources() components::AngularAcceleration()); if (!this->enableContactDetection(true)) { - gymppError << "Failed to enable contact detection" << std::endl; + sError << "Failed to enable contact detection" << std::endl; return false; } diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp index 8e19a4e12..c4b995cbc 100644 --- a/cpp/scenario/gazebo/src/Model.cpp +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -135,7 +135,7 @@ bool Model::initialize(const ignition::gazebo::Entity modelEntity, // Check that the model is valid if (!pImpl->model.Valid(*ecm)) { - gymppError << "The model entity is not valid" << std::endl; + sError << "The model entity is not valid" << std::endl; return false; } @@ -144,25 +144,23 @@ bool Model::initialize(const ignition::gazebo::Entity modelEntity, bool Model::createECMResources() { - gymppMessage << "Model: [" << pImpl->modelEntity << "] " << this->name() - << std::endl; + sMessage << "Model: [" << pImpl->modelEntity << "] " << this->name() + << std::endl; // Create required link resources - gymppMessage << "Links:" << std::endl; + sMessage << "Links:" << std::endl; for (const auto& linkName : this->linkNames()) { if (!this->getLink(linkName)->createECMResources()) { - gymppError << "Failed to initialize ECM link resources" - << std::endl; + sError << "Failed to initialize ECM link resources" << std::endl; return false; } } // Create required model resources - gymppMessage << "Joints:" << std::endl; + sMessage << "Joints:" << std::endl; for (const auto& jointName : this->jointNames()) { if (!this->getJoint(jointName)->createECMResources()) { - gymppError << "Failed to initialize ECM joint resources" - << std::endl; + sError << "Failed to initialize ECM joint resources" << std::endl; return false; } } @@ -503,8 +501,8 @@ double Model::controllerPeriod() const bool Model::setControllerPeriod(const double period) { if (period <= 0) { - gymppError << "The controller period must be greater than zero" - << std::endl; + sError << "The controller period must be greater than zero" + << std::endl; return false; } @@ -805,19 +803,19 @@ bool Model::setBaseFrame(const std::string& frameName) if (std::find(linkNames.begin(), linkNames.end(), frameName) == linkNames.end()) { - gymppError + sError << "Failed to set the model base on the frame of nonexistent link '" << frameName << "'" << std::endl; return false; } if (frameName == this->baseFrame()) { - gymppDebug << "Frame '" << baseFrame() - << "' is already the current model base" << std::endl; + sDebug << "Frame '" << baseFrame() + << "' is already the current model base" << std::endl; return true; } - gymppError << "Changing the base link is not yet supported" << std::endl; + sError << "Changing the base link is not yet supported" << std::endl; throw exceptions::NotImplementedError(__FUNCTION__); } @@ -945,7 +943,7 @@ bool Model::resetBasePose(const std::array& position, ignition::gazebo::components::ParentEntity(pImpl->modelEntity)); if (canonicalLinkEntity == ignition::gazebo::kNullEntity) { - gymppError << "Failed to get entity of canonical link" << std::endl; + sError << "Failed to get entity of canonical link" << std::endl; return false; } @@ -1234,9 +1232,9 @@ bool Model::Impl::setJointDataSerialized( } if (data.size() != expectedDOFs) { - gymppError << "The size of the forces does not match the considered " - "joint's DOFs" - << std::endl; + sError << "The size of the forces does not match the considered " + "joint's DOFs" + << std::endl; return false; } @@ -1245,8 +1243,8 @@ bool Model::Impl::setJointDataSerialized( for (auto& joint : model->joints(jointNames)) { for (size_t dof = 0; dof < joint->dofs(); ++dof) { if (!setJointData(joint, *it++, dof)) { - gymppError << "Failed to set force of joint '" << joint->name() - << "'" << std::endl; + sError << "Failed to set force of joint '" << joint->name() + << "'" << std::endl; return false; } } diff --git a/cpp/scenario/gazebo/src/World.cpp b/cpp/scenario/gazebo/src/World.cpp index d3cf0518f..7a2c6f2d4 100644 --- a/cpp/scenario/gazebo/src/World.cpp +++ b/cpp/scenario/gazebo/src/World.cpp @@ -160,7 +160,7 @@ bool World::setPhysicsEngine(const PhysicsEngine engine) } if (!this->insertWorldPlugin(libName, className)) { - gymppError << "Failed to insert the physics plugin" << std::endl; + sError << "Failed to insert the physics plugin" << std::endl; return false; } @@ -185,8 +185,8 @@ bool World::setGravity(const std::array& gravity) utils::steadyClockDurationToDouble(simTimeAtWorldCreation); if (this->time() > simTimeAtWorldCreationInSeconds) { - gymppError << "Physics already processed the world and its" - << "parameters cannot be modified" << std::endl; + sError << "Physics already processed the world and its" + << "parameters cannot be modified" << std::endl; return false; } @@ -277,7 +277,7 @@ bool World::insertModel(const std::string& modelFile, } if (modelSdfRoot->ModelCount() != 1) { - gymppError << "The SDF file contains more than one model" << std::endl; + sError << "The SDF file contains more than one model" << std::endl; return false; } @@ -300,9 +300,9 @@ bool World::insertModel(const std::string& modelFile, const std::vector modelNames = this->modelNames(); if (std::find(modelNames.begin(), modelNames.end(), finalModelEntityName) != modelNames.end()) { - gymppError << "Failed to insert model '" << finalModelEntityName - << "'. Another entity with the same name already exists." - << std::endl; + sError << "Failed to insert model '" << finalModelEntityName + << "'. Another entity with the same name already exists." + << std::endl; return false; } @@ -317,12 +317,12 @@ bool World::insertModel(const std::string& modelFile, // receiving the model entity name, they receive the model sdf name. if (!utils::renameSDFModel( *modelSdfRoot, finalModelEntityName, ModelIndex)) { - gymppError << "Failed to rename SDF model" << std::endl; + sError << "Failed to rename SDF model" << std::endl; return false; } if (utils::verboseFromEnvironment()) { - gymppDebug << "Inserting a model from the following SDF:" << std::endl; + sDebug << "Inserting a model from the following SDF:" << std::endl; std::cout << modelSdfRoot->Element()->ToString("") << std::endl; } @@ -366,7 +366,7 @@ bool World::insertModel(const std::string& modelFile, // Create required model resources. This call prepares all the necessary // components in the ECM to make our bindings work. if (!model->createECMResources()) { - gymppError << "Failed to initialize ECM model resources" << std::endl; + sError << "Failed to initialize ECM model resources" << std::endl; return false; } @@ -381,14 +381,14 @@ bool World::removeModel(const std::string& modelName) ignition::gazebo::components::ParentEntity(pImpl->worldEntity)); if (modelEntity == ignition::gazebo::kNullEntity) { - gymppError << "Model '" << modelName << "' not found in the world" - << std::endl; + sError << "Model '" << modelName << "' not found in the world" + << std::endl; return false; } // Request the removal of the model - gymppDebug << "Requesting removal of entity [" << modelEntity << "]" - << std::endl; + sDebug << "Requesting removal of entity [" << modelEntity << "]" + << std::endl; pImpl->sdfEntityCreator->RequestRemoveEntity(modelEntity); // Remove the cached model diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp index 39c874b03..865e176a5 100644 --- a/cpp/scenario/gazebo/src/helpers.cpp +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -53,10 +53,10 @@ utils::getSdfRootFromFile(const std::string& sdfFileName) auto errors = root->Load(sdfFileName); if (!errors.empty()) { - gymppError << "Failed to load sdf file " << sdfFileName << std::endl; + sError << "Failed to load sdf file " << sdfFileName << std::endl; for (const auto& error : errors) { - gymppError << error << std::endl; + sError << error << std::endl; } return {}; } @@ -74,10 +74,10 @@ utils::getSdfRootFromString(const std::string& sdfString) auto errors = root->LoadSdfString(sdfString); if (!errors.empty()) { - gymppError << "Failed to load sdf string" << std::endl; + sError << "Failed to load sdf string" << std::endl; for (const auto& error : errors) { - gymppError << error << std::endl; + sError << error << std::endl; } return {}; } @@ -237,22 +237,21 @@ sdf::World utils::renameSDFWorld(const sdf::World& world, auto errors = renamedWorld.Load(renamedWorldElement); if (!errors.empty()) { - gymppError << "Failed to create the World from the element" - << std::endl; + sError << "Failed to create the World from the element" << std::endl; for (const auto& error : errors) { - gymppError << error << std::endl; + sError << error << std::endl; } return {}; } if (renamedWorld.Name() != newWorldName) { - gymppError << "Failed to rename the world" << std::endl; + sError << "Failed to rename the world" << std::endl; return {}; } if (renamedWorld.ModelCount() != initialNrOfModels) { - gymppError << "Failed to copy all models to the new world" << std::endl; + sError << "Failed to copy all models to the new world" << std::endl; return {}; } @@ -290,12 +289,12 @@ bool utils::renameSDFModel(sdf::Root& sdfRoot, sdfRoot.Element()->InsertElement(renamedModel); if (sdfRoot.ModelCount() != initialNrOfModels) { - gymppError << "Failed to rename SDF model" << std::endl; + sError << "Failed to rename SDF model" << std::endl; return false; } if (!sdfRoot.ModelNameExists(newModelName)) { - gymppError << "Failed to insert renamed model in SDF root" << std::endl; + sError << "Failed to insert renamed model in SDF root" << std::endl; return false; } @@ -309,20 +308,20 @@ bool utils::updateSDFPhysics(sdf::Root& sdfRoot, const size_t worldIndex) { if (rtf <= 0) { - gymppError << "Invalid RTF value (" << rtf << ")" << std::endl; + sError << "Invalid RTF value (" << rtf << ")" << std::endl; return false; } if (maxStepSize <= 0) { - gymppError << "Invalid physics max step size (" << maxStepSize << ")" - << std::endl; + sError << "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; + sError << "Found more than one physics profile" << std::endl; return false; } @@ -390,7 +389,7 @@ sdf::JointType utils::toSdf(const scenario::base::JointType type) sdfType = sdf::JointType::BALL; break; default: - gymppError << "Joint type not recognized" << std::endl; + sError << "Joint type not recognized" << std::endl; sdfType = sdf::JointType::INVALID; break; } @@ -416,7 +415,7 @@ scenario::base::JointType utils::fromSdf(const sdf::JointType sdfType) type = base::JointType::Ball; break; default: - gymppError << "Joint type not recognized" << std::endl; + sError << "Joint type not recognized" << std::endl; type = base::JointType::Invalid; break; } @@ -476,7 +475,7 @@ WorldPtr utils::getParentWorld(ignition::gazebo::EntityComponentManager* ecm, auto world = std::make_shared(); if (!world->initialize(worldEntity, ecm, eventManager)) { - gymppError << "Failed to initialize world" << std::endl; + sError << "Failed to initialize world" << std::endl; return nullptr; } @@ -493,7 +492,7 @@ ModelPtr utils::getParentModel(ignition::gazebo::EntityComponentManager* ecm, auto model = std::make_shared(); if (!model->initialize(modelEntity, ecm, eventManager)) { - gymppError << "Failed to initialize model" << std::endl; + sError << "Failed to initialize model" << std::endl; return nullptr; } diff --git a/cpp/scenario/gazebo/src/signals.cpp b/cpp/scenario/gazebo/src/signals.cpp index a6b2295f7..a9e89872b 100644 --- a/cpp/scenario/gazebo/src/signals.cpp +++ b/cpp/scenario/gazebo/src/signals.cpp @@ -57,17 +57,16 @@ void SignalManager::ExecuteCallback(SignalType type) { std::lock_guard lock(detail::SignalManagerMutex); - gymppDebug << "Received interrupt signal " << Impl::ToString(type) - << std::endl; + sDebug << "Received interrupt signal " << Impl::ToString(type) << std::endl; auto callback = SignalManager::Instance().getCallback(type); if (callback) { - gymppDebug << "Found signal callback" << std::endl; + sDebug << "Found signal callback" << std::endl; callback(type); return; } - gymppDebug << "No callback found" << std::endl; + sDebug << "No callback found" << std::endl; } SignalManager& SignalManager::Instance() @@ -92,8 +91,8 @@ SignalManager::setCallback(const SignalType type, { SignalCallback oldCallback = this->getCallback(type); - gymppDebug << "Setting callback for signal " << Impl::ToString(type) - << std::endl; + sDebug << "Setting callback for signal " << Impl::ToString(type) + << std::endl; std::signal(type, SignalManager::ExecuteCallback); pImpl->callbacks[type] = callback; diff --git a/cpp/scenario/gazebo/src/utils.cpp b/cpp/scenario/gazebo/src/utils.cpp index c52dd1ea4..d7f43b854 100644 --- a/cpp/scenario/gazebo/src/utils.cpp +++ b/cpp/scenario/gazebo/src/utils.cpp @@ -57,7 +57,7 @@ void utils::setVerbosity(const int level) std::string utils::findSdfFile(const std::string& fileName) { if (fileName.empty()) { - gymppError << "The SDF file name is empty" << std::endl; + sError << "The SDF file name is empty" << std::endl; return {}; } @@ -69,9 +69,9 @@ std::string utils::findSdfFile(const std::string& fileName) std::string sdfFilePath = systemPaths.FindFile(fileName); if (sdfFilePath.empty()) { - gymppError << "Failed to find " << fileName << std::endl; - gymppError << "Check that it is part of IGN_GAZEBO_RESOURCE_PATH" - << std::endl; + sError << "Failed to find " << fileName << std::endl; + sError << "Check that it is part of IGN_GAZEBO_RESOURCE_PATH" + << std::endl; return {}; } @@ -112,7 +112,7 @@ std::string utils::getModelNameFromSdf(const std::string& fileName, std::string absFileName = findSdfFile(fileName); if (absFileName.empty()) { - gymppError << "Failed to find file " << fileName << std::endl; + sError << "Failed to find file " << fileName << std::endl; return {}; } @@ -123,14 +123,14 @@ std::string utils::getModelNameFromSdf(const std::string& fileName, } if (root->ModelCount() == 0) { - gymppError << "Didn't find any model in file " << fileName << std::endl; + sError << "Didn't find any model in file " << fileName << std::endl; return {}; } if (modelIndex >= root->ModelCount()) { - gymppError << "Model with index " << modelIndex - << " not found. The model has only " << root->ModelCount() - << " model(s)" << std::endl; + sError << "Model with index " << modelIndex + << " not found. The model has only " << root->ModelCount() + << " model(s)" << std::endl; return {}; } @@ -143,7 +143,7 @@ std::string utils::getWorldNameFromSdf(const std::string& fileName, std::string absFileName = findSdfFile(fileName); if (absFileName.empty()) { - gymppError << "Failed to find file " << fileName << std::endl; + sError << "Failed to find file " << fileName << std::endl; return {}; } @@ -154,14 +154,14 @@ std::string utils::getWorldNameFromSdf(const std::string& fileName, } if (root->WorldCount() == 0) { - gymppError << "Didn't find any world in file " << fileName << std::endl; + sError << "Didn't find any world in file " << fileName << std::endl; return {}; } if (worldIndex >= root->WorldCount()) { - gymppError << "Model with index " << worldIndex - << " not found. The model has only " << root->WorldCount() - << " model(s)" << std::endl; + sError << "Model with index " << worldIndex + << " not found. The model has only " << root->WorldCount() + << " model(s)" << std::endl; return {}; } @@ -205,7 +205,7 @@ std::string utils::getModelFileFromFuel(const std::string& URI, modelFilePath = fuel_tools::fetchResource(URI); if (modelFilePath.empty()) { - gymppError << "Failed to download Fuel model" << std::endl; + sError << "Failed to download Fuel model" << std::endl; return {}; } } @@ -215,7 +215,7 @@ std::string utils::getModelFileFromFuel(const std::string& URI, auto result = client.CachedModel(common::URI(URI), modelFilePath); if (result.Type() != fuel_tools::ResultType::FETCH_ALREADY_EXISTS) { - gymppError << "Fuel model not found locally" << std::endl; + sError << "Fuel model not found locally" << std::endl; return {}; } } @@ -225,8 +225,8 @@ std::string utils::getModelFileFromFuel(const std::string& URI, std::string modelFile = common::joinPaths(modelFilePath, "model.sdf"); if (!common::isFile(modelFile)) { - gymppError << "The model was downloaded from Fuel but it was not found " - << "in the filesystem" << std::endl; + sError << "The model was downloaded from Fuel but it was not found " + << "in the filesystem" << std::endl; return {}; } @@ -253,8 +253,8 @@ std::string utils::getInstallPrefix() #ifdef GYMIGNITION_CMAKE_INSTALL_PREFIX return GYMIGNITION_CMAKE_INSTALL_PREFIX; #else - gymppDebug << "User installation detected. The install prefix " - << "could be detected from the Python module path." << std::endl; + sDebug << "User installation detected. The install prefix " + << "could be detected from the Python module path." << std::endl; return ""; #endif } diff --git a/cpp/scenario/plugins/ControllerRunner/ControllerRunner.cpp b/cpp/scenario/plugins/ControllerRunner/ControllerRunner.cpp index e42712381..e3b43705d 100644 --- a/cpp/scenario/plugins/ControllerRunner/ControllerRunner.cpp +++ b/cpp/scenario/plugins/ControllerRunner/ControllerRunner.cpp @@ -112,19 +112,19 @@ void ControllerRunner::Configure(const ignition::gazebo::Entity& entity, pImpl->model = std::make_shared(); if (!pImpl->model->initialize(entity, &ecm, &eventMgr)) { - gymppError << "Failed to initialize model for controller" << std::endl; + sError << "Failed to initialize model for controller" << std::endl; return; } if (!pImpl->model->valid()) { - gymppError << "Failed to create a model from Entity [" << entity << "]" - << std::endl; + sError << "Failed to create a model from Entity [" << entity << "]" + << std::endl; return; } if (sdf->GetName() != "plugin") { - gymppError << "Received context does not contain the element" - << std::endl; + sError << "Received context does not contain the element" + << std::endl; return; } @@ -148,12 +148,12 @@ void ControllerRunner::Configure(const ignition::gazebo::Entity& entity, ControllersFactory::Instance().get(controllerContext, pImpl->model); if (!pImpl->controller) { - gymppError << "Failed to find controller in the factory" << std::endl; + sError << "Failed to find controller in the factory" << std::endl; return; } if (!pImpl->controller->initialize()) { - gymppError << "Failed to initialize the controller" << std::endl; + sError << "Failed to initialize the controller" << std::endl; pImpl->controller = nullptr; return; } @@ -172,12 +172,12 @@ void ControllerRunner::Configure(const ignition::gazebo::Entity& entity, // Here we check if the controller inherits from the supported interfaces. if (!(pImpl->controllerInterfaces.base || pImpl->controllerInterfaces.joints)) { - gymppWarning << "Failed to find any of the supported interfaces to set " - << "controller references" << std::endl; + sWarning << "Failed to find any of the supported interfaces to set " + << "controller references" << std::endl; return; } - gymppDebug << "Controller successfully initialized" << std::endl; + sDebug << "Controller successfully initialized" << std::endl; } void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, @@ -200,8 +200,8 @@ void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, } if (!pImpl->controller) { - gymppError << "The controller was not initialized successfully" - << std::endl; + sError << "The controller was not initialized successfully" + << std::endl; return; } @@ -249,26 +249,23 @@ void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, try { if (!pImpl->updateAllSupportedReferences(ecm)) { - gymppError << "Failed to update supported references" - << std::endl; + sError << "Failed to update supported references" << std::endl; return; } } catch (const exceptions::ComponentNotFound& e) { - gymppDebug << "Controller references not yet available" - << std::endl; - gymppDebug << e.what(); - gymppWarning << "[t=" - << utils::steadyClockDurationToDouble(info.simTime) - << "] The controller is not stepping" << std::endl; + sDebug << "Controller references not yet available" << std::endl; + sDebug << e.what(); + sWarning << "[t=" + << utils::steadyClockDurationToDouble(info.simTime) + << "] The controller is not stepping" << std::endl; return; } if (pImpl->controllerInterfaces.useModel && !pImpl->controllerInterfaces.useModel->updateStateFromModel()) { - gymppError - << "Failed to update controller state from internal model" - << std::endl; + sError << "Failed to update controller state from internal model" + << std::endl; return; } @@ -279,7 +276,7 @@ void ControllerRunner::PreUpdate(const ignition::gazebo::UpdateInfo& info, // Step the controller if (pImpl->referencesHaveBeenSet && !pImpl->controller->step(info.dt)) { - gymppError << "Failed to step the controller" << std::endl; + sError << "Failed to step the controller" << std::endl; return; } } @@ -291,12 +288,12 @@ bool ControllerRunner::Impl::updateAllSupportedReferences( if (controllerInterfaces.base) { if (!updateBaseReferencesfromECM(ecm)) { - gymppError << "Failed to update base references" << std::endl; + sError << "Failed to update base references" << std::endl; ok = false; } else { if (!controllerInterfaces.base->setBaseReferences(baseReferences)) { - gymppError << "Failed to set base references" << std::endl; + sError << "Failed to set base references" << std::endl; ok = false; } } @@ -304,13 +301,13 @@ bool ControllerRunner::Impl::updateAllSupportedReferences( if (controllerInterfaces.joints) { if (!updateJointReferencesfromECM(ecm)) { - gymppError << "Failed to update joint references" << std::endl; + sError << "Failed to update joint references" << std::endl; ok = false; } else { if (!controllerInterfaces.joints->setJointReferences( jointReferences)) { - gymppError << "Failed to set joint references" << std::endl; + sError << "Failed to set joint references" << std::endl; ok = false; } } @@ -389,7 +386,7 @@ bool ControllerRunner::Impl::updateJointReferencesfromECM( void ControllerRunner::Impl::printControllerContext( const std::shared_ptr context) const { - gymppDebug << "SDF elements received by the controller:" << std::endl; + sDebug << "SDF elements received by the controller:" << std::endl; std::cout << context->ToString("") << std::endl; } diff --git a/cpp/scenario/plugins/ControllerRunner/ControllersFactory.cpp b/cpp/scenario/plugins/ControllerRunner/ControllersFactory.cpp index 970751817..934ab1087 100644 --- a/cpp/scenario/plugins/ControllerRunner/ControllersFactory.cpp +++ b/cpp/scenario/plugins/ControllerRunner/ControllersFactory.cpp @@ -76,13 +76,13 @@ ControllersFactory::get(const sdf::ElementPtr context, scenario::gazebo::ModelPtr model) { if (!Impl::ContextValid(context)) { - gymppError << "Controller context not valid" << std::endl; + sError << "Controller context not valid" << std::endl; return nullptr; } std::string controllerName; context->GetAttribute("name")->Get(controllerName); - gymppDebug << "Found context for " << controllerName << std::endl; + sDebug << "Found context for " << controllerName << std::endl; if (controllerName == "ComputedTorqueFixedBase") { @@ -94,8 +94,7 @@ ControllersFactory::get(const sdf::ElementPtr context, ok = ok && context->HasElement("gravity"); if (!ok) { - gymppError << "Controller context has missing elements" - << std::endl; + sError << "Controller context has missing elements" << std::endl; return nullptr; } @@ -108,7 +107,7 @@ ControllersFactory::get(const sdf::ElementPtr context, "joints", context); if (gravity.size() != 3) { - gymppError << "Parsed gravity does not have three elements"; + sError << "Parsed gravity does not have three elements"; return nullptr; } @@ -131,23 +130,22 @@ bool ControllersFactory::Impl::ContextValid(const sdf::ElementPtr context) { // Check that the context is a element if (context->GetName() != "controller") { - gymppError << "The first element of the context must be " - << std::endl; + sError << "The first element of the context must be " + << std::endl; return false; } // Check that there is only one element if (context->GetNextElement("controller")) { - gymppError - << "Found multiple elements in controller context" - << std::endl; + sError << "Found multiple elements in controller context" + << std::endl; return false; } // Check that there is a name attribute: if (!context->HasAttribute("name")) { - gymppError << "Failed to find 'name' attribute in element" - << std::endl; + sError << "Failed to find 'name' attribute in element" + << std::endl; return false; } @@ -160,8 +158,7 @@ T ControllersFactory::Impl::GetElementValueAs( const sdf::ElementPtr parentContext) { if (!parentContext->HasElement(elementName)) { - gymppError << "Failed to find element <" << elementName << ">" - << std::endl; + sError << "Failed to find element <" << elementName << ">" << std::endl; return {}; } @@ -171,8 +168,8 @@ T ControllersFactory::Impl::GetElementValueAs( sdf::ParamPtr value = element->GetValue(); if (!value) { - gymppError << "Failed to get value of element <" << elementName << ">" - << std::endl; + sError << "Failed to get value of element <" << elementName << ">" + << std::endl; return {}; } diff --git a/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp index d2ee6e3bf..dbb8ebf66 100644 --- a/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp +++ b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp @@ -50,7 +50,7 @@ ECMProvider::ECMProvider() ECMProvider::~ECMProvider() { ECMSingleton::Instance().clean(); - gymppDebug << "Destroying the ECMProvider" << std::endl; + sDebug << "Destroying the ECMProvider" << std::endl; }; void ECMProvider::Configure(const ignition::gazebo::Entity& entity, @@ -60,8 +60,8 @@ void ECMProvider::Configure(const ignition::gazebo::Entity& entity, { if (!ecm.EntityHasComponentType( entity, ignition::gazebo::components::World().TypeId())) { - gymppError << "The ECMProvider plugin was not inserted " - << "in a world element" << std::endl; + sError << "The ECMProvider plugin was not inserted " + << "in a world element" << std::endl; return; } @@ -69,19 +69,19 @@ void ECMProvider::Configure(const ignition::gazebo::Entity& entity, ignition::gazebo::components::Name>(&ecm, entity); if (ECMSingleton::Instance().hasWorld(worldName)) { - gymppWarning << "Resources of world " << worldName - << " already inserted" << std::endl; + sWarning << "Resources of world " << worldName << " already inserted" + << std::endl; return; } if (!ECMSingleton::Instance().storePtrs(&ecm, &eventMgr, worldName)) { - gymppError << "Failed to store resources of world " << worldName << " [" - << entity << "]" << std::endl; + sError << "Failed to store resources of world " << worldName << " [" + << entity << "]" << std::endl; return; } - gymppDebug << "World '" << worldName - << "' successfully processed by ECMProvider" << std::endl; + sDebug << "World '" << worldName + << "' successfully processed by ECMProvider" << std::endl; } IGNITION_ADD_PLUGIN(scenario::plugins::gazebo::ECMProvider, diff --git a/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp index 4cd59a90b..1676223b5 100644 --- a/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp +++ b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp @@ -79,8 +79,8 @@ void ECMSingleton::clean(const std::string& worldName) } if (!this->hasWorld(worldName)) { - gymppError << "Resources of world " << worldName << " not found" - << std::endl; + sError << "Resources of world " << worldName << " not found" + << std::endl; return; } @@ -92,7 +92,7 @@ bool ECMSingleton::valid(const std::string& worldName) const std::unique_lock lock(pImpl->mutex); if (!this->hasWorld(worldName)) { - gymppDebug << "World" << worldName << " not found" << std::endl; + sDebug << "World" << worldName << " not found" << std::endl; return false; } @@ -139,14 +139,14 @@ ECMSingleton::getEventManager(const std::string& worldName) const std::unique_lock lock(pImpl->mutex); if (!this->hasWorld(worldName)) { - gymppError << "Resources of world " << worldName << " not found" - << std::endl; + sError << "Resources of world " << worldName << " not found" + << std::endl; return nullptr; } if (!this->valid(worldName)) { - gymppError << "Resources of world " << worldName << " not valid" - << std::endl; + sError << "Resources of world " << worldName << " not valid" + << std::endl; return nullptr; } @@ -159,14 +159,14 @@ ECMSingleton::getECM(const std::string& worldName) const std::unique_lock lock(pImpl->mutex); if (!this->hasWorld(worldName)) { - gymppError << "Resources of world " << worldName << " not found" - << std::endl; + sError << "Resources of world " << worldName << " not found" + << std::endl; return nullptr; } if (!this->valid(worldName)) { - gymppError << "Resources of world " << worldName << " not valid" - << std::endl; + sError << "Resources of world " << worldName << " not valid" + << std::endl; return nullptr; } @@ -178,21 +178,21 @@ bool ECMSingleton::storePtrs(ignition::gazebo::EntityComponentManager* ecm, const std::string& worldName) { if (!ecm || !eventMgr) { - gymppError << "The pointer to the ECM or EventManager is not valid" - << std::endl; + sError << "The pointer to the ECM or EventManager is not valid" + << std::endl; return false; } if (worldName.empty()) { - gymppError << "The world name is empty" << std::endl; + sError << "The world name is empty" << std::endl; return false; } std::unique_lock lock(pImpl->mutex); if (this->hasWorld(worldName)) { - gymppError << "Resources of world " << worldName - << " have been already stored" << std::endl; + sError << "Resources of world " << worldName + << " have been already stored" << std::endl; return false; } diff --git a/cpp/scenario/plugins/JointController/JointController.cpp b/cpp/scenario/plugins/JointController/JointController.cpp index 6bceb0794..e937d52d7 100644 --- a/cpp/scenario/plugins/JointController/JointController.cpp +++ b/cpp/scenario/plugins/JointController/JointController.cpp @@ -87,13 +87,13 @@ void JointController::Configure( // Create a model and check its validity if (!pImpl->model->initialize(entity, &ecm, &eventMgr)) { - gymppError << "Failed to initialize model for controller" << std::endl; + sError << "Failed to initialize model for controller" << std::endl; return; } if (!pImpl->model->valid()) { - gymppError << "Failed to create a model from Entity [" << entity << "]" - << std::endl; + sError << "Failed to create a model from Entity [" << entity << "]" + << std::endl; return; } } @@ -195,9 +195,8 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, info.dt, positionTarget, position)) { - gymppError << "Failed to run PID controller of joint " - << joint->name() << " [" << jointEntity << "]" - << std::endl; + sError << "Failed to run PID controller of joint " << joint->name() + << " [" << jointEntity << "]" << std::endl; } } @@ -227,9 +226,8 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, info.dt, velocityTarget, velocity)) { - gymppError << "Failed to run PID controller of joint " - << joint->name() << " [" << jointEntity << "]" - << std::endl; + sError << "Failed to run PID controller of joint " << joint->name() + << " [" << jointEntity << "]" << std::endl; } } } @@ -261,8 +259,8 @@ bool JointController::Impl::runPIDController( } if (!joint.setGeneralizedForceTarget(force)) { - gymppError << "Failed to set force of joint " << joint.name() - << std::endl; + sError << "Failed to set force of joint " << joint.name() + << std::endl; return false; } return true; @@ -270,8 +268,8 @@ bool JointController::Impl::runPIDController( case base::JointType::Fixed: case base::JointType::Ball: case base::JointType::Invalid: - gymppWarning << "Type of joint '" << joint.name() - << " not supported" << std::endl; + sWarning << "Type of joint '" << joint.name() << " not supported" + << std::endl; return true; }