diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 7f420e09..ec0ed4c8 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -86,6 +86,16 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) +add_executable(gzserver src/gzserver.cpp) +ament_target_dependencies(gzserver + rclcpp + std_msgs +) +target_link_libraries(gzserver + ${GZ_TARGET_PREFIX}-sim${GZ_SIM_VER}::core +) +ament_target_dependencies(gzserver std_msgs) + configure_file( launch/gz_sim.launch.py.in launch/gz_sim.launch.py.configured @@ -105,6 +115,10 @@ install(TARGETS create DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS + gzserver + DESTINATION lib/${PROJECT_NAME} +) install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp new file mode 100644 index 00000000..3cdd43c9 --- /dev/null +++ b/ros_gz_sim/src/gzserver.cpp @@ -0,0 +1,68 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// 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. + +#include +#include +#include +#include +#include + +int main(int _argc, char ** _argv) +{ + using namespace gz; + auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); + auto node = rclcpp::Node::make_shared("gzserver"); + auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); + + common::Console::SetVerbosity(4); + sim::ServerConfig server_config; + + + 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( + node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + return -1; + } + + // (azeey) Testing if a plugin can be loaded along side the defaults. Not working yet. + + // + // + sdf::Plugin imu_sdf_plugin; + imu_sdf_plugin.SetName("gz::sim::systems::Imu"); + imu_sdf_plugin.SetFilename("gz-sim-imu-system"); + sim::SystemLoader loader; + auto imu_plugin = loader.LoadPlugin(imu_sdf_plugin); + std::cout << imu_plugin.value() << std::endl; + + gz::sim::Server server(server_config); + server.RunOnce(); + if (!server.AddSystem(*imu_plugin)) + { + RCLCPP_ERROR( + node->get_logger(), "IMU system not added"); + + } + server.Run(true /*blocking*/, 0, false /*paused*/); +}