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.

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

Physics: update calls to use sdf::Errors output #1157

Merged
merged 7 commits into from
Mar 29, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
8 changes: 8 additions & 0 deletions include/sdf/Physics.hh
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,14 @@ namespace sdf
/// \return SDF element pointer with updated physics values.
public: sdf::ElementPtr ToElement() const;

/// \brief Create and return an SDF element filled with data from this
/// physics.
/// Note that parameter passing functionality is not captured with this
/// function.
/// \param[out] _errors Vector of errors.
/// \return SDF element pointer with updated physics values.
public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const;

/// \brief Private data pointer.
GZ_UTILS_IMPL_PTR(dataPtr)
};
Expand Down
35 changes: 24 additions & 11 deletions src/Physics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -86,10 +86,10 @@ Errors Physics::Load(sdf::ElementPtr _sdf)
}

this->dataPtr->isDefault =
_sdf->Get<bool>("default", this->dataPtr->isDefault).first;
_sdf->Get<bool>(errors, "default", this->dataPtr->isDefault).first;

std::pair<std::string, bool> stringPair =
_sdf->Get<std::string>("type", this->dataPtr->type);
_sdf->Get<std::string>(errors, "type", this->dataPtr->type);
if (!stringPair.second)
{
errors.push_back({ErrorCode::ELEMENT_MISSING,
Expand All @@ -99,7 +99,7 @@ Errors Physics::Load(sdf::ElementPtr _sdf)
this->dataPtr->type = stringPair.first;

std::pair<double, bool> doublePair =
_sdf->Get<double>("max_step_size", this->dataPtr->stepSize);
_sdf->Get<double>(errors, "max_step_size", this->dataPtr->stepSize);
if (!doublePair.second)
{
errors.push_back({ErrorCode::ELEMENT_MISSING,
Expand All @@ -108,7 +108,8 @@ Errors Physics::Load(sdf::ElementPtr _sdf)
}
this->dataPtr->stepSize = doublePair.first;

doublePair = _sdf->Get<double>("real_time_factor", this->dataPtr->rtf);
doublePair = _sdf->Get<double>(
errors, "real_time_factor", this->dataPtr->rtf);
if (!doublePair.second)
{
errors.push_back({ErrorCode::ELEMENT_MISSING,
Expand All @@ -118,7 +119,7 @@ Errors Physics::Load(sdf::ElementPtr _sdf)
this->dataPtr->rtf = doublePair.first;

this->dataPtr->maxContacts =
_sdf->Get<int>("max_contacts", this->dataPtr->maxContacts).first;
_sdf->Get<int>(errors, "max_contacts", this->dataPtr->maxContacts).first;

return errors;
}
Expand Down Expand Up @@ -203,17 +204,29 @@ void Physics::SetMaxContacts(int _maxContacts)

/////////////////////////////////////////////////
sdf::ElementPtr Physics::ToElement() const
{
sdf::Errors errors;
auto result = this->ToElement(errors);
sdf::throwOrPrintErrors(errors);
return result;
}

/////////////////////////////////////////////////
sdf::ElementPtr Physics::ToElement(sdf::Errors &_errors) const
{
sdf::ElementPtr elem(new sdf::Element);
sdf::initFile("physics.sdf", elem);

elem->GetAttribute("name")->Set(this->Name());
elem->GetAttribute("default")->Set(this->IsDefault());
elem->GetAttribute("type")->Set(this->EngineType());
elem->GetAttribute("name")->Set(this->Name(), _errors);
elem->GetAttribute("default")->Set(this->IsDefault(), _errors);
elem->GetAttribute("type")->Set(this->EngineType(), _errors);

elem->GetElement("max_step_size")->Set(this->MaxStepSize());
elem->GetElement("real_time_factor")->Set(this->RealTimeFactor());
elem->GetElement("max_contacts")->Set(this->MaxContacts());
elem->GetElement("max_step_size", _errors)->Set(
_errors, this->MaxStepSize());
elem->GetElement("real_time_factor", _errors)->Set(
_errors, this->RealTimeFactor());
elem->GetElement("max_contacts", _errors)->Set(
_errors, this->MaxContacts());

/// \todo(nkoenig) Support engine specific parameters.

Expand Down
46 changes: 46 additions & 0 deletions src/Physics_TEST.cc
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <gtest/gtest.h>
#include "sdf/Physics.hh"
#include "test_utils.hh"

/////////////////////////////////////////////////
TEST(DOMPhysics, DefaultConstruction)
Expand Down Expand Up @@ -131,3 +132,48 @@ TEST(DOMPhysics, ToElement)
EXPECT_DOUBLE_EQ(physics.RealTimeFactor(), physics2.RealTimeFactor());
EXPECT_EQ(physics.MaxContacts(), physics2.MaxContacts());
}

/////////////////////////////////////////////////
TEST(DOMPhysics, ToElementErrorOutput)
{
std::stringstream buffer;
sdf::testing::RedirectConsoleStream redir(
sdf::Console::Instance()->GetMsgStream(), &buffer);

#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(false);
sdf::testing::ScopeExit revertSetQuiet(
[]
{
sdf::Console::Instance()->SetQuiet(true);
});
#endif

sdf::Physics physics;
sdf::Errors errors;
physics.SetName("my-bullet-engine");
physics.SetDefault(true);
physics.SetEngineType("bullet");
physics.SetMaxStepSize(0.1);
physics.SetRealTimeFactor(20.4);
physics.SetMaxContacts(42);

sdf::ElementPtr elem = physics.ToElement(errors);
EXPECT_TRUE(errors.empty());
ASSERT_NE(nullptr, elem);

sdf::Physics physics2;
errors = physics2.Load(elem);
EXPECT_TRUE(errors.empty());

// verify values after loading the element back
EXPECT_EQ(physics.Name(), physics2.Name());
EXPECT_EQ(physics.IsDefault(), physics2.IsDefault());
EXPECT_EQ(physics.EngineType(), physics2.EngineType());
EXPECT_DOUBLE_EQ(physics.MaxStepSize(), physics2.MaxStepSize());
EXPECT_DOUBLE_EQ(physics.RealTimeFactor(), physics2.RealTimeFactor());
EXPECT_EQ(physics.MaxContacts(), physics2.MaxContacts());

// Check nothing has been printed
EXPECT_TRUE(buffer.str().empty()) << buffer.str();
}