Skip to content

Commit

Permalink
Expose header for GzServer
Browse files Browse the repository at this point in the history
Signed-off-by: Patrick Roncagliolo <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
roncapat and ahcorde committed Jan 8, 2025
1 parent 23f7250 commit 8ad6eb0
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 44 deletions.
44 changes: 44 additions & 0 deletions ros_gz_sim/include/ros_gz_sim/gzserver.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
// Copyright 2025 Open Source Robotics Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROS_GZ_SIM__GZSERVER_HPP_
#define ROS_GZ_SIM__GZSERVER_HPP_

#include <thread>
#include <rclcpp/node.hpp>

// ROS node that executes a gz-sim Server given a world SDF file or string.
namespace ros_gz_sim
{
class GzServer : public rclcpp::Node
{
public:
// Class constructor.
explicit GzServer(const rclcpp::NodeOptions & options);

public:
// Class destructor.
~GzServer();

public:
/// \brief Run the gz sim server.
void OnStart();

private:
/// \brief We don't want to block the ROS thread.
std::thread thread_;
};
} // namespace ros_gz_sim

#endif // ROS_GZ_SIM__GZSERVER_HPP_
77 changes: 33 additions & 44 deletions ros_gz_sim/src/gzserver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,68 +13,57 @@
// limitations under the License.

#include <functional>
#include <thread>
#include <gz/common/Console.hh>
#include <gz/sim/Server.hh>
#include <gz/sim/SystemLoader.hh>
#include <gz/sim/ServerConfig.hh>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>

// ROS node that executes a gz-sim Server given a world SDF file or string.
#include <ros_gz_sim/gzserver.hpp>

namespace ros_gz_sim
{
class GzServer : public rclcpp::Node

GzServer::GzServer(const rclcpp::NodeOptions & options)
: Node("gzserver", options)
{
public:
// Class constructor.
explicit GzServer(const rclcpp::NodeOptions & options)
: Node("gzserver", options)
{
thread_ = std::thread(std::bind(&GzServer::OnStart, this));
}
thread_ = std::thread(std::bind(&GzServer::OnStart, this));
}

public:
// Class destructor.
~GzServer()
{
// Make sure to join the thread on shutdown.
if (thread_.joinable()) {
thread_.join();
}
GzServer::~GzServer()
{
// Make sure to join the thread on shutdown.
if (thread_.joinable()) {
thread_.join();
}
}

public:
/// \brief Run the gz sim server.
void OnStart()
{
auto world_sdf_file = this->declare_parameter("world_sdf_file", "");
auto world_sdf_string = this->declare_parameter("world_sdf_string", "");

gz::common::Console::SetVerbosity(4);
gz::sim::ServerConfig server_config;
void GzServer::OnStart()
{
auto world_sdf_file = this->declare_parameter("world_sdf_file", "");
auto world_sdf_string = this->declare_parameter("world_sdf_string", "");

if (!world_sdf_file.empty()) {
server_config.SetSdfFile(world_sdf_file);
} else if (!world_sdf_string.empty()) {
server_config.SetSdfString(world_sdf_string);
} else {
RCLCPP_ERROR(
this->get_logger(),
"Must specify either 'world_sdf_file' or 'world_sdf_string'");
rclcpp::shutdown();
return;
}
gz::common::Console::SetVerbosity(4);
gz::sim::ServerConfig server_config;

gz::sim::Server server(server_config);
server.Run(true /*blocking*/, 0, false /*paused*/);
if (!world_sdf_file.empty()) {
server_config.SetSdfFile(world_sdf_file);
} else if (!world_sdf_string.empty()) {
server_config.SetSdfString(world_sdf_string);
} else {
RCLCPP_ERROR(
this->get_logger(),
"Must specify either 'world_sdf_file' or 'world_sdf_string'");
rclcpp::shutdown();
return;
}

private:
/// \brief We don't want to block the ROS thread.
std::thread thread_;
};
gz::sim::Server server(server_config);
server.Run(true /*blocking*/, 0, false /*paused*/);
rclcpp::shutdown();
}

} // namespace ros_gz_sim

RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer)

0 comments on commit 8ad6eb0

Please sign in to comment.