Skip to content

Commit daa2460

Browse files
iche033iche033
authored
Merge pull request #2760 from gazebosim/breakout_server_9
Port Server changes from gz-sim8 to gz-sim9 (#2638 and #2754)
2 parents c1f32b1 + b675b4d commit daa2460

File tree

4 files changed

+172
-139
lines changed

4 files changed

+172
-139
lines changed

‎src/Server.cc

+2-120
Original file line numberDiff line numberDiff line change
@@ -33,31 +33,12 @@
3333
#include "gz/sim/Server.hh"
3434
#include "gz/sim/Util.hh"
3535

36-
#include "MeshInertiaCalculator.hh"
3736
#include "ServerPrivate.hh"
3837
#include "SimulationRunner.hh"
3938

4039
using namespace gz;
4140
using namespace sim;
4241

43-
/// \brief This struct provides access to the default world.
44-
struct DefaultWorld
45-
{
46-
/// \brief Get the default world as a string.
47-
/// Plugins will be loaded from the server.config file.
48-
/// \return An SDF string that contains the default world.
49-
public: static std::string &World()
50-
{
51-
static std::string world = std::string("<?xml version='1.0'?>"
52-
"<sdf version='1.6'>"
53-
"<world name='default'>") +
54-
"</world>"
55-
"</sdf>";
56-
57-
return world;
58-
}
59-
};
60-
6142
/////////////////////////////////////////////////
6243
Server::Server(const ServerConfig &_config)
6344
: dataPtr(new ServerPrivate)
@@ -96,107 +77,8 @@ Server::Server(const ServerConfig &_config)
9677

9778
addResourcePaths();
9879

99-
sdf::Errors errors;
100-
101-
switch (_config.Source())
102-
{
103-
// Load a world if specified. Check SDF string first, then SDF file
104-
case ServerConfig::SourceType::kSdfRoot:
105-
{
106-
this->dataPtr->sdfRoot = _config.SdfRoot()->Clone();
107-
gzmsg << "Loading SDF world from SDF DOM.\n";
108-
break;
109-
}
110-
111-
case ServerConfig::SourceType::kSdfString:
112-
{
113-
std::string msg = "Loading SDF string. ";
114-
if (_config.SdfFile().empty())
115-
{
116-
msg += "File path not available.\n";
117-
}
118-
else
119-
{
120-
msg += "File path [" + _config.SdfFile() + "].\n";
121-
}
122-
gzmsg << msg;
123-
sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
124-
sdfParserConfig.SetStoreResolvedURIs(true);
125-
sdfParserConfig.SetCalculateInertialConfiguration(
126-
sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
127-
errors = this->dataPtr->sdfRoot.LoadSdfString(
128-
_config.SdfString(), sdfParserConfig);
129-
this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
130-
break;
131-
}
132-
133-
case ServerConfig::SourceType::kSdfFile:
134-
{
135-
std::string filePath = resolveSdfWorldFile(_config.SdfFile(),
136-
_config.ResourceCache());
137-
138-
if (filePath.empty())
139-
{
140-
gzerr << "Failed to find world [" << _config.SdfFile() << "]"
141-
<< std::endl;
142-
return;
143-
}
144-
145-
gzmsg << "Loading SDF world file[" << filePath << "].\n";
146-
147-
sdf::Root sdfRoot;
148-
sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
149-
sdfParserConfig.SetStoreResolvedURIs(true);
150-
sdfParserConfig.SetCalculateInertialConfiguration(
151-
sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
152-
153-
MeshInertiaCalculator meshInertiaCalculator;
154-
sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator);
155-
// \todo(nkoenig) Async resource download.
156-
// This call can block for a long period of time while
157-
// resources are downloaded. Blocking here causes the GUI to block with
158-
// a black screen (search for "Async resource download" in
159-
// 'src/gui_main.cc'.
160-
errors = sdfRoot.Load(filePath, sdfParserConfig);
161-
if (errors.empty() || _config.BehaviorOnSdfErrors() !=
162-
ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
163-
{
164-
if (sdfRoot.Model() == nullptr) {
165-
this->dataPtr->sdfRoot = std::move(sdfRoot);
166-
}
167-
else
168-
{
169-
// If the specified file only contains a model, load the default
170-
// world and add the model to it.
171-
errors = this->dataPtr->sdfRoot.LoadSdfString(
172-
DefaultWorld::World(), sdfParserConfig);
173-
sdf::World *world = this->dataPtr->sdfRoot.WorldByIndex(0);
174-
if (world == nullptr) {
175-
return;
176-
}
177-
world->AddModel(*sdfRoot.Model());
178-
if (errors.empty() || _config.BehaviorOnSdfErrors() !=
179-
ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
180-
{
181-
errors = this->dataPtr->sdfRoot.UpdateGraphs();
182-
}
183-
}
184-
}
185-
186-
this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
187-
break;
188-
}
189-
190-
case ServerConfig::SourceType::kNone:
191-
default:
192-
{
193-
gzmsg << "Loading default world.\n";
194-
// Load an empty world.
195-
/// \todo(nkoenig) Add a "AddWorld" function to sdf::Root.
196-
errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World());
197-
break;
198-
}
199-
}
80+
// Loads the SDF root object based on values in a ServerConfig object.
81+
sdf::Errors errors = this->dataPtr->LoadSdfRootHelper(_config);
20082

20183
if (!errors.empty())
20284
{

‎src/ServerPrivate.cc

+117
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,14 @@
1414
* limitations under the License.
1515
*
1616
*/
17+
#include "MeshInertiaCalculator.hh"
1718
#include "ServerPrivate.hh"
1819

1920
#include <tinyxml2.h>
2021

22+
#include <string>
23+
#include <utility>
24+
2125
#include <gz/msgs/boolean.pb.h>
2226
#include <gz/msgs/server_control.pb.h>
2327
#include <gz/msgs/stringmsg.pb.h>
@@ -596,3 +600,116 @@ std::string ServerPrivate::FetchResourceUri(const common::URI &_uri)
596600
{
597601
return this->FetchResource(_uri.Str());
598602
}
603+
604+
//////////////////////////////////////////////////
605+
sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config)
606+
{
607+
sdf::Errors errors;
608+
609+
sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig();
610+
sdfParserConfig.SetStoreResolvedURIs(true);
611+
sdfParserConfig.SetCalculateInertialConfiguration(
612+
sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
613+
MeshInertiaCalculator meshInertiaCalculator;
614+
sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator);
615+
616+
switch (_config.Source())
617+
{
618+
// Load a world if specified. Check SDF string first, then SDF file
619+
case ServerConfig::SourceType::kSdfRoot:
620+
{
621+
this->sdfRoot = _config.SdfRoot()->Clone();
622+
gzmsg << "Loading SDF world from SDF DOM.\n";
623+
break;
624+
}
625+
626+
case ServerConfig::SourceType::kSdfString:
627+
{
628+
std::string msg = "Loading SDF string. ";
629+
if (_config.SdfFile().empty())
630+
{
631+
msg += "File path not available.\n";
632+
}
633+
else
634+
{
635+
msg += "File path [" + _config.SdfFile() + "].\n";
636+
}
637+
gzmsg << msg;
638+
errors = this->sdfRoot.LoadSdfString(
639+
_config.SdfString(), sdfParserConfig);
640+
this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
641+
break;
642+
}
643+
644+
case ServerConfig::SourceType::kSdfFile:
645+
{
646+
std::string filePath = resolveSdfWorldFile(_config.SdfFile(),
647+
_config.ResourceCache());
648+
649+
if (filePath.empty())
650+
{
651+
std::string errStr = "Failed to find world ["
652+
+ _config.SdfFile() + "]";
653+
gzerr << errStr << std::endl;
654+
errors.push_back({sdf::ErrorCode::FILE_READ, errStr});
655+
return errors;
656+
}
657+
658+
gzmsg << "Loading SDF world file[" << filePath << "].\n";
659+
660+
sdf::Root sdfRootLocal;
661+
// \todo(nkoenig) Async resource download.
662+
// This call can block for a long period of time while
663+
// resources are downloaded. Blocking here causes the GUI to block with
664+
// a black screen (search for "Async resource download" in
665+
// 'src/gui_main.cc'.
666+
errors = sdfRootLocal.Load(filePath, sdfParserConfig);
667+
if (errors.empty() || _config.BehaviorOnSdfErrors() !=
668+
ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
669+
{
670+
if (sdfRootLocal.Model() == nullptr) {
671+
this->sdfRoot = std::move(sdfRootLocal);
672+
}
673+
else
674+
{
675+
sdf::World defaultWorld;
676+
defaultWorld.SetName("default");
677+
678+
// If the specified file only contains a model, load the default
679+
// world and add the model to it.
680+
errors = this->sdfRoot.AddWorld(defaultWorld);
681+
sdf::World *world = this->sdfRoot.WorldByIndex(0);
682+
if (world == nullptr) {
683+
errors.push_back({sdf::ErrorCode::FATAL_ERROR,
684+
"sdf::World pointer is null"});
685+
return errors;
686+
}
687+
world->AddModel(*sdfRootLocal.Model());
688+
if (errors.empty() || _config.BehaviorOnSdfErrors() !=
689+
ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY)
690+
{
691+
errors = this->sdfRoot.UpdateGraphs();
692+
}
693+
}
694+
}
695+
696+
this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig);
697+
break;
698+
}
699+
700+
case ServerConfig::SourceType::kNone:
701+
default:
702+
{
703+
gzmsg << "Loading default world.\n";
704+
705+
sdf::World defaultWorld;
706+
defaultWorld.SetName("default");
707+
708+
// Load an empty world.
709+
errors = this->sdfRoot.AddWorld(defaultWorld);
710+
break;
711+
}
712+
}
713+
714+
return errors;
715+
}

‎src/ServerPrivate.hh

+6
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,12 @@ namespace gz
9999
/// \return Path to the downloaded resource, empty on error.
100100
public: std::string FetchResourceUri(const common::URI &_uri);
101101

102+
/// \brief Helper function that loads an SDF root object based on
103+
/// values in a ServerConfig object.
104+
/// \param[in] _config Server config to read from.
105+
/// \return Set of SDF errors.
106+
public: sdf::Errors LoadSdfRootHelper(const ServerConfig &_config);
107+
102108
/// \brief Signal handler callback
103109
/// \param[in] _sig The signal number
104110
private: void OnSignal(int _sig);

‎test/integration/mesh_inertia_calculation.cc

+47-19
Original file line numberDiff line numberDiff line change
@@ -51,20 +51,39 @@ class MeshInertiaCalculationTest : public InternalFixture<::testing::Test>
5151
{
5252
};
5353

54-
TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
54+
/// \brief Load an SDF world and run mesh inertia tests. Two tests are run
55+
/// one after another: 1) the server is launched with path to SDF file, and
56+
/// 2) ther server is launched from an sdf string.
57+
/// \param[in] _path Path to SDF
58+
/// \param[in] _testFunc Test function that checks mesh inertia values
59+
void loadSdfAndTest(const std::string &_path,
60+
std::function<void(const gz::sim::ServerConfig &)> _testFunc)
5561
{
56-
size_t kIter = 100u;
57-
58-
// Start server and run.
62+
// Test mesh inertial calculator with sdf loaded from file
5963
gz::sim::ServerConfig serverConfig;
60-
serverConfig.SetSdfFile(common::joinPaths(
61-
PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf"));
62-
64+
serverConfig.SetSdfFile(_path);
6365
common::setenv(
6466
"GZ_SIM_RESOURCE_PATH",
6567
common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"));
68+
_testFunc(serverConfig);
6669

67-
gz::sim::Server server(serverConfig);
70+
71+
// Test mesh inertial calculator with sdf loaded from string
72+
std::ifstream sdfFile(_path);
73+
std::stringstream buffer;
74+
buffer << sdfFile.rdbuf();
75+
gz::sim::ServerConfig serverConfigSdfStr;
76+
serverConfigSdfStr.SetSdfString(buffer.str());
77+
_testFunc(serverConfigSdfStr);
78+
}
79+
80+
void cylinderColladaMeshInertiaCalculation(
81+
const gz::sim::ServerConfig &_serverConfig)
82+
{
83+
size_t kIter = 100u;
84+
85+
// Start server and run.
86+
gz::sim::Server server(_serverConfig);
6887

6988
// Create a system just to get the ECM
7089
EntityComponentManager *ecm;
@@ -127,21 +146,20 @@ TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
127146
EXPECT_EQ(link.WorldInertialPose(*ecm).value(), gz::math::Pose3d::Zero);
128147
}
129148

130-
TEST(MeshInertiaCalculationTest,
131-
CylinderColladaMeshWithNonCenterOriginInertiaCalculation)
149+
TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
150+
{
151+
std::string sdfFilePath = common::joinPaths(
152+
PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf");
153+
loadSdfAndTest(sdfFilePath, cylinderColladaMeshInertiaCalculation);
154+
}
155+
156+
void cylinderColladaMeshWithNonCenterOriginInertiaCalculation(
157+
const gz::sim::ServerConfig &_serverConfig)
132158
{
133159
size_t kIter = 100u;
134160

135161
// Start server and run.
136-
gz::sim::ServerConfig serverConfig;
137-
serverConfig.SetSdfFile(common::joinPaths(
138-
PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf"));
139-
140-
common::setenv(
141-
"GZ_SIM_RESOURCE_PATH",
142-
common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"));
143-
144-
gz::sim::Server server(serverConfig);
162+
gz::sim::Server server(_serverConfig);
145163

146164
// Create a system just to get the ECM
147165
EntityComponentManager *ecm;
@@ -207,6 +225,16 @@ TEST(MeshInertiaCalculationTest,
207225
// the center of mass (inertial pose) will be 1m above the ground
208226
EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
209227
gz::math::Pose3d(0, 0, 1, 0, 0, 0));
228+
229+
}
230+
231+
TEST(MeshInertiaCalculationTest,
232+
CylinderColladaMeshWithNonCenterOriginInertiaCalculation)
233+
{
234+
std::string sdfFilePath = common::joinPaths(
235+
PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf");
236+
237+
loadSdfAndTest(sdfFilePath, cylinderColladaMeshInertiaCalculation);
210238
}
211239

212240
TEST(MeshInertiaCalculationTest, CylinderColladaOptimizedMeshInertiaCalculation)

0 commit comments

Comments
 (0)