From 5d3d4061038a3510e7591214f75d216196a3c7e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 21:41:34 +0100 Subject: [PATCH 01/25] Bridge as a gz system MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 10 +++++++--- ros_gz_bridge/src/ros_gz_bridge.cpp | 1 + ros_gz_image/CMakeLists.txt | 1 + ros_gz_sim/CMakeLists.txt | 17 +++++++++++++++++ ros_gz_sim/package.xml | 1 + 5 files changed, 27 insertions(+), 3 deletions(-) diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 4d0e4057..25f4d2c5 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -148,7 +148,9 @@ ament_target_dependencies(${bridge_lib} ) target_include_directories(${bridge_lib} - PUBLIC include + PUBLIC + "$" + "$" PRIVATE src ${generated_path} ) @@ -162,13 +164,13 @@ rclcpp_components_register_node( PLUGIN ros_gz_bridge::RosGzBridge EXECUTABLE bridge_node) -install(TARGETS ${bridge_lib} +install(TARGETS ${bridge_lib} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install( - DIRECTORY include/ + DIRECTORY include/${PROJECT_NAME} DESTINATION include ) @@ -348,6 +350,8 @@ if(BUILD_TESTING) endif() +ament_export_targets(export_${PROJECT_NAME}) + ament_export_dependencies( rclcpp ${BRIDGE_MESSAGE_TYPES} diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index f542ee90..c6ea56bb 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -44,6 +44,7 @@ void RosGzBridge::spin() std::string config_file; this->get_parameter("config_file", config_file); if (!config_file.empty()) { + std::cerr << "Reading parameters" << std::endl; auto entries = readFromYamlFile(config_file); for (const auto & entry : entries) { this->add_bridge(entry); diff --git a/ros_gz_image/CMakeLists.txt b/ros_gz_image/CMakeLists.txt index 545b0b27..986e6282 100644 --- a/ros_gz_image/CMakeLists.txt +++ b/ros_gz_image/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(image_transport REQUIRED) find_package(ros_gz_bridge REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) # TODO(CH3): Deprecated. Remove on tock. diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 29d2ea28..21c2ad84 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,6 +12,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ros_gz_bridge REQUIRED) find_package(std_msgs REQUIRED) # TODO(CH3): Deprecated. Remove on tock. @@ -127,6 +129,21 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) +# ROS Gz bridge +add_library(ROSGzPlugin SHARED src/ros_gz.cc) +target_link_libraries(ROSGzPlugin PUBLIC + gz-sim${GZ_SIM_VER}::core + rclcpp::rclcpp + ros_gz_bridge::ros_gz_bridge_lib +) +target_include_directories(ROSGzPlugin PUBLIC + $ + $ +) +install( + TARGETS ROSGzPlugin + DESTINATION lib) + configure_file( launch/gz_sim.launch.py.in launch/gz_sim.launch.py.configured diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 181c3199..a8bb1bae 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -16,6 +16,7 @@ ament_index_python libgflags-dev rclcpp + ros_gz_bridge std_msgs From 2882c873f991e6dc9603ee10950c65993e0ef9fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 21:44:28 +0100 Subject: [PATCH 02/25] Extra files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_bridge/src/ros_gz_bridge.cpp | 1 - ros_gz_sim/include/ros_gz_sim/ros_gz.hh | 49 +++++++++++ ros_gz_sim/src/ros_gz.cc | 100 +++++++++++++++++++++++ ros_gz_sim_demos/config/full.yaml | 18 ++++ ros_gz_sim_demos/launch/ros_gz.launch.py | 44 ++++++++++ ros_gz_sim_demos/worlds/ros_gz.sdf | 85 +++++++++++++++++++ 6 files changed, 296 insertions(+), 1 deletion(-) create mode 100644 ros_gz_sim/include/ros_gz_sim/ros_gz.hh create mode 100644 ros_gz_sim/src/ros_gz.cc create mode 100644 ros_gz_sim_demos/config/full.yaml create mode 100644 ros_gz_sim_demos/launch/ros_gz.launch.py create mode 100644 ros_gz_sim_demos/worlds/ros_gz.sdf diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp index c6ea56bb..f542ee90 100644 --- a/ros_gz_bridge/src/ros_gz_bridge.cpp +++ b/ros_gz_bridge/src/ros_gz_bridge.cpp @@ -44,7 +44,6 @@ void RosGzBridge::spin() std::string config_file; this->get_parameter("config_file", config_file); if (!config_file.empty()) { - std::cerr << "Reading parameters" << std::endl; auto entries = readFromYamlFile(config_file); for (const auto & entry : entries) { this->add_bridge(entry); diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hh b/ros_gz_sim/include/ros_gz_sim/ros_gz.hh new file mode 100644 index 00000000..bea2c09f --- /dev/null +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2024 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_PLUGIN_HH_ +#define ROS_GZ_PLUGIN_HH_ + +#include +#include +#include +#include + +namespace ros_gz_sim +{ +/// \brief ToDo +class ROSGzPlugin + : public gz::sim::System, + public gz::sim::ISystemConfigure +{ + // \brief Constructor. + public: ROSGzPlugin(); + + /// \brief Destructor. + public: ~ROSGzPlugin() override; + + // Documentation inherited. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + /// \brief Private data pointer. + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) +}; +} +#endif diff --git a/ros_gz_sim/src/ros_gz.cc b/ros_gz_sim/src/ros_gz.cc new file mode 100644 index 00000000..58932148 --- /dev/null +++ b/ros_gz_sim/src/ros_gz.cc @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2024 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. + * +*/ + +#include +#include +#include + +#include +#include +#include +#include + +#include "ros_gz.hh" + +using namespace gz; +using namespace ros_gz_sim; + +/// \brief Private ROSGzPlugin data class. +class ROSGzPlugin::Implementation +{ + /// \brief The ROS 2 <--> Gz bridge. + public: std::shared_ptr bridge; + + /// \brief The ROS 2 executor. + public: std::shared_ptr exec; + + /// \brief A thread to call spin and not block the Gazebo thread. + public: std::thread thread; +}; + +////////////////////////////////////////////////// +ROSGzPlugin::ROSGzPlugin() + : System(), dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +ROSGzPlugin::~ROSGzPlugin() +{ + this->dataPtr->exec->cancel(); + this->dataPtr->thread.join(); +} + +////////////////////////////////////////////////// +void ROSGzPlugin::Configure(const sim::Entity &/*_entity*/, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &/*_ecm*/, sim::EventManager &/*_eventMgr*/) +{ + // Ensure that ROS is setup. + if (!rclcpp::ok()) + rclcpp::init(0, nullptr); + + if (!_sdf->HasElement("config_file")) + { + gzerr << "No found. Plugin disabled." << std::endl; + return; + } + + // Sanity check: Make sure that the config file exists and it's a file. + std::filesystem::path configFile = _sdf->Get("config_file"); + if (!std::filesystem::is_regular_file(configFile)) + { + gzerr << "[" << configFile << "] is not a regular file. Plugin disabled" + << std::endl; + return; + } + + // Create the bridge passing the parameters as rclcpp::NodeOptions(). + this->dataPtr->bridge = std::make_shared( + rclcpp::NodeOptions().append_parameter_override("config_file", configFile)); + + // Create the executor. + this->dataPtr->exec = + std::make_shared(); + this->dataPtr->exec->add_node(this->dataPtr->bridge); + + // Spin in a separate thread to not block Gazebo. + this->dataPtr->thread = std::thread([this](){this->dataPtr->exec->spin();}); +} + +GZ_ADD_PLUGIN(ROSGzPlugin, + sim::System, + ROSGzPlugin::ISystemConfigure) + +GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") diff --git a/ros_gz_sim_demos/config/full.yaml b/ros_gz_sim_demos/config/full.yaml new file mode 100644 index 00000000..ac838101 --- /dev/null +++ b/ros_gz_sim_demos/config/full.yaml @@ -0,0 +1,18 @@ +# Full set of configurations +- ros_topic_name: "ros_chatter" + gz_topic_name: "gz_chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + subscriber_queue: 5 + publisher_queue: 6 + lazy: true + direction: ROS_TO_GZ + +- ros_topic_name: "ros_chatter" + gz_topic_name: "gz_chatter" + ros_type_name: "std_msgs/msg/String" + gz_type_name: "ignition.msgs.StringMsg" + subscriber_queue: 10 + publisher_queue: 20 + lazy: false + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/ros_gz.launch.py b/ros_gz_sim_demos/launch/ros_gz.launch.py new file mode 100644 index 00000000..f1b7e0a5 --- /dev/null +++ b/ros_gz_sim_demos/launch/ros_gz.launch.py @@ -0,0 +1,44 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), + ), + launch_arguments={'gz_args': PathJoinSubstitution([ + pkg_ros_gz_sim_demos, + 'worlds', + 'ros_gz.sdf' + ])}.items(), + ) + + return LaunchDescription([ + gazebo, + ]) diff --git a/ros_gz_sim_demos/worlds/ros_gz.sdf b/ros_gz_sim_demos/worlds/ros_gz.sdf new file mode 100644 index 00000000..b1d66dbe --- /dev/null +++ b/ros_gz_sim_demos/worlds/ros_gz.sdf @@ -0,0 +1,85 @@ + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + true + 0 0 0 0 0 0 + + package://ros_gz_sim_demos/models/vehicle + + + + + + + true + true + true + 1 + + + + vehicle/odom + vehicle + + + + /home/caguero/ros_gz_ws/src/ros_gz/ros_gz_sim_demos/config/full.yaml + + + + + From 4921a073bf1b8f8a46fc2c3164f40c56b2c382cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 22:11:50 +0100 Subject: [PATCH 03/25] Rename MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 2 +- ros_gz_sim/include/ros_gz_sim/{ros_gz.hh => ros_gz.hpp} | 0 ros_gz_sim/src/{ros_gz.cc => ros_gz.cpp} | 2 +- ros_gz_sim_demos/launch/ros_gz.launch.py | 7 ++----- 4 files changed, 4 insertions(+), 7 deletions(-) rename ros_gz_sim/include/ros_gz_sim/{ros_gz.hh => ros_gz.hpp} (100%) rename ros_gz_sim/src/{ros_gz.cc => ros_gz.cpp} (99%) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 21c2ad84..cc65ee63 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -130,7 +130,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) # ROS Gz bridge -add_library(ROSGzPlugin SHARED src/ros_gz.cc) +add_library(ROSGzPlugin SHARED src/ros_gz.cpp) target_link_libraries(ROSGzPlugin PUBLIC gz-sim${GZ_SIM_VER}::core rclcpp::rclcpp diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hh b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp similarity index 100% rename from ros_gz_sim/include/ros_gz_sim/ros_gz.hh rename to ros_gz_sim/include/ros_gz_sim/ros_gz.hpp diff --git a/ros_gz_sim/src/ros_gz.cc b/ros_gz_sim/src/ros_gz.cpp similarity index 99% rename from ros_gz_sim/src/ros_gz.cc rename to ros_gz_sim/src/ros_gz.cpp index 58932148..926b017c 100644 --- a/ros_gz_sim/src/ros_gz.cc +++ b/ros_gz_sim/src/ros_gz.cpp @@ -24,7 +24,7 @@ #include #include -#include "ros_gz.hh" +#include "ros_gz.hpp" using namespace gz; using namespace ros_gz_sim; diff --git a/ros_gz_sim_demos/launch/ros_gz.launch.py b/ros_gz_sim_demos/launch/ros_gz.launch.py index f1b7e0a5..6cf90335 100644 --- a/ros_gz_sim_demos/launch/ros_gz.launch.py +++ b/ros_gz_sim_demos/launch/ros_gz.launch.py @@ -16,12 +16,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node - +from launch.substitutions import PathJoinSubstitution def generate_launch_description(): From 9ddd65296b072305aef7956d303572534212e186 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 23:04:29 +0100 Subject: [PATCH 04/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 56 ++++++++++++------------ ros_gz_sim/src/ros_gz.cpp | 46 ++++++++++--------- 2 files changed, 49 insertions(+), 53 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index bea2c09f..1a232d1a 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -1,22 +1,19 @@ -/* - * Copyright (C) 2024 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. - * -*/ +// Copyright 2024 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_PLUGIN_HH_ -#define ROS_GZ_PLUGIN_HH_ +#ifndef ROS_GZ_SIM__ROS_GZ_HPP_ +#define ROS_GZ_SIM__ROS_GZ_HPP_ #include #include @@ -30,20 +27,21 @@ class ROSGzPlugin : public gz::sim::System, public gz::sim::ISystemConfigure { - // \brief Constructor. - public: ROSGzPlugin(); + public: + // \brief Constructor. + ROSGzPlugin(); - /// \brief Destructor. - public: ~ROSGzPlugin() override; + /// \brief Destructor. + virtual ~ROSGzPlugin() override; - // Documentation inherited. - public: void Configure(const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &_eventMgr) override; + // Documentation inherited. + void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; /// \brief Private data pointer. GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; -} -#endif +} // namespace ros_gz_sim +#endif // ROS_GZ_SIM__ROS_GZ_HPP_ diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 926b017c..8b2dbb98 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -1,19 +1,16 @@ -/* - * Copyright (C) 2024 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. - * -*/ +// 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 @@ -26,8 +23,8 @@ #include "ros_gz.hpp" -using namespace gz; -using namespace ros_gz_sim; +namespace ros_gz_sim +{ /// \brief Private ROSGzPlugin data class. class ROSGzPlugin::Implementation @@ -44,7 +41,7 @@ class ROSGzPlugin::Implementation ////////////////////////////////////////////////// ROSGzPlugin::ROSGzPlugin() - : System(), dataPtr(utils::MakeUniqueImpl()) + : System(), dataPtr(gz::utils::MakeUniqueImpl()) { } @@ -56,9 +53,9 @@ ROSGzPlugin::~ROSGzPlugin() } ////////////////////////////////////////////////// -void ROSGzPlugin::Configure(const sim::Entity &/*_entity*/, +void ROSGzPlugin::Configure(const gz::sim::Entity &/*_entity*/, const std::shared_ptr &_sdf, - sim::EntityComponentManager &/*_ecm*/, sim::EventManager &/*_eventMgr*/) + gz::sim::EntityComponentManager &/*_ecm*/, gz::sim::EventManager &/*_eventMgr*/) { // Ensure that ROS is setup. if (!rclcpp::ok()) @@ -91,10 +88,11 @@ void ROSGzPlugin::Configure(const sim::Entity &/*_entity*/, // Spin in a separate thread to not block Gazebo. this->dataPtr->thread = std::thread([this](){this->dataPtr->exec->spin();}); } +} // namespace ros_gz_sim -GZ_ADD_PLUGIN(ROSGzPlugin, - sim::System, - ROSGzPlugin::ISystemConfigure) +GZ_ADD_PLUGIN(ros_gz_sim::ROSGzPlugin, + gz::sim::System, + ros_gz_sim::ROSGzPlugin::ISystemConfigure) GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, "ros_gz_sim::ROSGzPlugin") From 758aa219bcc6d2548640bdf20ec37fe219295ca4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 23:21:39 +0100 Subject: [PATCH 05/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 4 ++-- ros_gz_sim/src/ros_gz.cpp | 13 +++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index 1a232d1a..5e8dcf49 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -27,12 +27,12 @@ class ROSGzPlugin : public gz::sim::System, public gz::sim::ISystemConfigure { - public: + public: // \brief Constructor. ROSGzPlugin(); /// \brief Destructor. - virtual ~ROSGzPlugin() override; + ~ROSGzPlugin() override; // Documentation inherited. void Configure(const gz::sim::Entity &_entity, diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 8b2dbb98..d1ff3b8e 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -29,14 +29,15 @@ namespace ros_gz_sim /// \brief Private ROSGzPlugin data class. class ROSGzPlugin::Implementation { - /// \brief The ROS 2 <--> Gz bridge. - public: std::shared_ptr bridge; + public: + /// \brief The ROS 2 <--> Gz bridge. + std::shared_ptr bridge; - /// \brief The ROS 2 executor. - public: std::shared_ptr exec; + /// \brief The ROS 2 executor. + std::shared_ptr exec; - /// \brief A thread to call spin and not block the Gazebo thread. - public: std::thread thread; + /// \brief A thread to call spin and not block the Gazebo thread. + std::thread thread; }; ////////////////////////////////////////////////// From f1f5fa010abf4158158d243278c8f071bc235a78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 16 Feb 2024 23:52:05 +0100 Subject: [PATCH 06/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 24 ++++++------ ros_gz_sim/src/ros_gz.cpp | 47 +++++++++++++----------- ros_gz_sim_demos/launch/ros_gz.launch.py | 1 + 3 files changed, 38 insertions(+), 34 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index 5e8dcf49..b48732f9 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -24,21 +24,21 @@ namespace ros_gz_sim { /// \brief ToDo class ROSGzPlugin - : public gz::sim::System, - public gz::sim::ISystemConfigure + : public gz::sim::System, + public gz::sim::ISystemConfigure { - public: - // \brief Constructor. - ROSGzPlugin(); +public: + // \brief Constructor. + ROSGzPlugin(); - /// \brief Destructor. - ~ROSGzPlugin() override; + /// \brief Destructor. + ~ROSGzPlugin() override; - // Documentation inherited. - void Configure(const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &_eventMgr) override; + // Documentation inherited. + void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; /// \brief Private data pointer. GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index d1ff3b8e..7d5277f3 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -29,20 +29,20 @@ namespace ros_gz_sim /// \brief Private ROSGzPlugin data class. class ROSGzPlugin::Implementation { - public: - /// \brief The ROS 2 <--> Gz bridge. - std::shared_ptr bridge; +public: + /// \brief The ROS 2 <--> Gz bridge. + std::shared_ptr bridge; - /// \brief The ROS 2 executor. - std::shared_ptr exec; + /// \brief The ROS 2 executor. + std::shared_ptr exec; - /// \brief A thread to call spin and not block the Gazebo thread. - std::thread thread; + /// \brief A thread to call spin and not block the Gazebo thread. + std::thread thread; }; ////////////////////////////////////////////////// ROSGzPlugin::ROSGzPlugin() - : System(), dataPtr(gz::utils::MakeUniqueImpl()) +: System(), dataPtr(gz::utils::MakeUniqueImpl()) { } @@ -54,24 +54,25 @@ ROSGzPlugin::~ROSGzPlugin() } ////////////////////////////////////////////////// -void ROSGzPlugin::Configure(const gz::sim::Entity &/*_entity*/, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &/*_ecm*/, gz::sim::EventManager &/*_eventMgr*/) +void ROSGzPlugin::Configure( + const gz::sim::Entity & /*_entity*/, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & /*_ecm*/, + gz::sim::EventManager & /*_eventMgr*/) { // Ensure that ROS is setup. - if (!rclcpp::ok()) + if (!rclcpp::ok()) { rclcpp::init(0, nullptr); + } - if (!_sdf->HasElement("config_file")) - { + if (!_sdf->HasElement("config_file")) { gzerr << "No found. Plugin disabled." << std::endl; return; } // Sanity check: Make sure that the config file exists and it's a file. std::filesystem::path configFile = _sdf->Get("config_file"); - if (!std::filesystem::is_regular_file(configFile)) - { + if (!std::filesystem::is_regular_file(configFile)) { gzerr << "[" << configFile << "] is not a regular file. Plugin disabled" << std::endl; return; @@ -87,13 +88,15 @@ void ROSGzPlugin::Configure(const gz::sim::Entity &/*_entity*/, this->dataPtr->exec->add_node(this->dataPtr->bridge); // Spin in a separate thread to not block Gazebo. - this->dataPtr->thread = std::thread([this](){this->dataPtr->exec->spin();}); + this->dataPtr->thread = std::thread([this]() {this->dataPtr->exec->spin();}); } } // namespace ros_gz_sim -GZ_ADD_PLUGIN(ros_gz_sim::ROSGzPlugin, - gz::sim::System, - ros_gz_sim::ROSGzPlugin::ISystemConfigure) +GZ_ADD_PLUGIN( + ros_gz_sim::ROSGzPlugin, + gz::sim::System, + ros_gz_sim::ROSGzPlugin::ISystemConfigure) -GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, - "ros_gz_sim::ROSGzPlugin") +GZ_ADD_PLUGIN_ALIAS( + ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") diff --git a/ros_gz_sim_demos/launch/ros_gz.launch.py b/ros_gz_sim_demos/launch/ros_gz.launch.py index 6cf90335..c1c00938 100644 --- a/ros_gz_sim_demos/launch/ros_gz.launch.py +++ b/ros_gz_sim_demos/launch/ros_gz.launch.py @@ -20,6 +20,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution + def generate_launch_description(): pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') From be153b65df2169dfbd119077e3ba75c6394ae342 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sat, 17 Feb 2024 00:07:45 +0100 Subject: [PATCH 07/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index b48732f9..35373e32 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -24,8 +24,8 @@ namespace ros_gz_sim { /// \brief ToDo class ROSGzPlugin - : public gz::sim::System, - public gz::sim::ISystemConfigure +: public gz::sim::System, + public gz::sim::ISystemConfigure { public: // \brief Constructor. @@ -35,10 +35,11 @@ class ROSGzPlugin ~ROSGzPlugin() override; // Documentation inherited. - void Configure(const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &_eventMgr) override; + void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; /// \brief Private data pointer. GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) From ac768779a63b6b30d01671cdf30aa9adc0115dde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Sat, 17 Feb 2024 00:24:19 +0100 Subject: [PATCH 08/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index 35373e32..27ca505b 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -24,7 +24,7 @@ namespace ros_gz_sim { /// \brief ToDo class ROSGzPlugin -: public gz::sim::System, + : public gz::sim::System, public gz::sim::ISystemConfigure { public: @@ -36,10 +36,10 @@ class ROSGzPlugin // Documentation inherited. void Configure( - const gz::sim::Entity &_entity, - const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &_eventMgr) override; + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; /// \brief Private data pointer. GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) From 3bc36bdad4540b5a39afcbdfb66ede79dd958f27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 18:07:12 +0100 Subject: [PATCH 09/25] Tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index cc65ee63..1658a001 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -22,6 +22,8 @@ if("$ENV{GZ_VERSION}" STREQUAL "" AND NOT "$ENV{IGNITION_VERSION}" STREQUAL "") set(ENV{GZ_VERSION} $ENV{IGNITION_VERSION}) endif() +set(GZ_TARGET_SIM gz-sim) + # Edifice if("$ENV{GZ_VERSION}" STREQUAL "edifice") find_package(ignition-math6 REQUIRED) @@ -37,6 +39,7 @@ if("$ENV{GZ_VERSION}" STREQUAL "edifice") set(GZ_SIM_VER ${ignition-gazebo5_VERSION_MAJOR}) set(GZ_TARGET_PREFIX ignition) + set(GZ_TARGET_SIM ignition-gazebo) macro(gz_find_package) ign_find_package(${ARGV}) endmacro() @@ -90,6 +93,7 @@ else() set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) set(GZ_TARGET_PREFIX ignition) + set(GZ_TARGET_SIM ignition-gazebo) message(STATUS "Compiling against Gazebo Fortress") macro(gz_find_package) @@ -132,7 +136,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ # ROS Gz bridge add_library(ROSGzPlugin SHARED src/ros_gz.cpp) target_link_libraries(ROSGzPlugin PUBLIC - gz-sim${GZ_SIM_VER}::core + ${GZ_TARGET_SIM}${GZ_SIM_VER}::core rclcpp::rclcpp ros_gz_bridge::ros_gz_bridge_lib ) From 6f05a128f50979c79f347624b6e8d23a25b07dfb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 20:42:03 +0100 Subject: [PATCH 10/25] Manual PIMPL MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 7 +++++-- ros_gz_sim/src/ros_gz.cpp | 5 ++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index 27ca505b..b5c038f5 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -17,11 +17,13 @@ #include #include -#include #include namespace ros_gz_sim { +// Private class. +class ROSGzPluginPrivate; + /// \brief ToDo class ROSGzPlugin : public gz::sim::System, @@ -41,8 +43,9 @@ class ROSGzPlugin gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) override; + /// \brief Private data pointer. - GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + std::unique_ptr dataPtr; }; } // namespace ros_gz_sim #endif // ROS_GZ_SIM__ROS_GZ_HPP_ diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 7d5277f3..8b7fb038 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -25,9 +25,8 @@ namespace ros_gz_sim { - /// \brief Private ROSGzPlugin data class. -class ROSGzPlugin::Implementation +class ROSGzPluginPrivate { public: /// \brief The ROS 2 <--> Gz bridge. @@ -42,7 +41,7 @@ class ROSGzPlugin::Implementation ////////////////////////////////////////////////// ROSGzPlugin::ROSGzPlugin() -: System(), dataPtr(gz::utils::MakeUniqueImpl()) +: System(), dataPtr(new ROSGzPluginPrivate()) { } From ebb6cab4620b98cd19768aa196ed22830c104821 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 21:03:22 +0100 Subject: [PATCH 11/25] Include MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 8b7fb038..e50de744 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include From bdcc825b9bb8e96c2ac1c8f3c2e7473f4de9e928 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 21:18:43 +0100 Subject: [PATCH 12/25] no gzerr MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index e50de744..6bc082f7 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -66,14 +65,14 @@ void ROSGzPlugin::Configure( } if (!_sdf->HasElement("config_file")) { - gzerr << "No found. Plugin disabled." << std::endl; + std::cerr << "No found. Plugin disabled." << std::endl; return; } // Sanity check: Make sure that the config file exists and it's a file. std::filesystem::path configFile = _sdf->Get("config_file"); if (!std::filesystem::is_regular_file(configFile)) { - gzerr << "[" << configFile << "] is not a regular file. Plugin disabled" + std::cerr << "[" << configFile << "] is not a regular file. Plugin disabled" << std::endl; return; } From 42aa8266b5a146b448aaa45b26557ef1a761993c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 21:39:06 +0100 Subject: [PATCH 13/25] tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 6bc082f7..2a7b68be 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -95,7 +95,3 @@ GZ_ADD_PLUGIN( ros_gz_sim::ROSGzPlugin, gz::sim::System, ros_gz_sim::ROSGzPlugin::ISystemConfigure) - -GZ_ADD_PLUGIN_ALIAS( - ros_gz_sim::ROSGzPlugin, - "ros_gz_sim::ROSGzPlugin") From 8d767e9d169f1320a3f66141c73fa893ffd06e1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 22:00:38 +0100 Subject: [PATCH 14/25] tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 2a7b68be..bc72c41d 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -73,7 +73,7 @@ void ROSGzPlugin::Configure( std::filesystem::path configFile = _sdf->Get("config_file"); if (!std::filesystem::is_regular_file(configFile)) { std::cerr << "[" << configFile << "] is not a regular file. Plugin disabled" - << std::endl; + << std::endl; return; } From 49c8811a0d6a999bd1587fd8a29a33ea18126381 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 23:28:58 +0100 Subject: [PATCH 15/25] tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index bc72c41d..7a0e810f 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -91,7 +91,22 @@ void ROSGzPlugin::Configure( } } // namespace ros_gz_sim +#if (IGNITION_GAZEBO_MAJOR_VERSION == 6) +IGNITION_ADD_PLUGIN( + ros_gz_sim::ROSGzPlugin, + gz::sim::System, + ros_gz_sim::ROSGzPlugin::ISystemConfigure) +#else GZ_ADD_PLUGIN( ros_gz_sim::ROSGzPlugin, gz::sim::System, ros_gz_sim::ROSGzPlugin::ISystemConfigure) +#endif + +#if (IGNITION_GAZEBO_MAJOR_VERSION == 6) +IGNITION_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") +#else +GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") +#endif From e2a5db06cd9a141a6f399ef2b3ab11df140a0e99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 23:43:19 +0100 Subject: [PATCH 16/25] tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 7a0e810f..64909088 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -109,4 +109,4 @@ IGNITION_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, #else GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, "ros_gz_sim::ROSGzPlugin") -#endif +#endif From a1952076ee3a1ed685d0cb4f7cc35f2980e66317 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 20 Feb 2024 23:57:59 +0100 Subject: [PATCH 17/25] tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 64909088..2d91bd6a 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -104,9 +104,11 @@ GZ_ADD_PLUGIN( #endif #if (IGNITION_GAZEBO_MAJOR_VERSION == 6) -IGNITION_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, - "ros_gz_sim::ROSGzPlugin") +IGNITION_ADD_PLUGIN_ALIAS( + ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") #else -GZ_ADD_PLUGIN_ALIAS(ros_gz_sim::ROSGzPlugin, - "ros_gz_sim::ROSGzPlugin") +GZ_ADD_PLUGIN_ALIAS( + ros_gz_sim::ROSGzPlugin, + "ros_gz_sim::ROSGzPlugin") #endif From fe132a1ffdab496a613a9cb5f7da3a6b68a3f68e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 22 Feb 2024 18:07:37 +0100 Subject: [PATCH 18/25] Tweaks MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/include/ros_gz_sim/ros_gz.hpp | 1 - ros_gz_sim/src/ros_gz.cpp | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp index b5c038f5..91973815 100644 --- a/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp +++ b/ros_gz_sim/include/ros_gz_sim/ros_gz.hpp @@ -43,7 +43,6 @@ class ROSGzPlugin gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventMgr) override; - /// \brief Private data pointer. std::unique_ptr dataPtr; }; diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 2d91bd6a..7817af37 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -13,8 +13,10 @@ // limitations under the License. #include +#include #include #include +#include #include #include From 8c5b845a4b59c2c77f72da44bca9d49321e76ffc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 22 Feb 2024 20:44:17 +0100 Subject: [PATCH 19/25] Find yaml file. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 3 +- ros_gz_sim/src/ros_gz.cpp | 20 ++++++----- ros_gz_sim_demos/CMakeLists.txt | 7 ++++ ros_gz_sim_demos/worlds/ros_gz.sdf | 58 +++++++++++++----------------- 4 files changed, 45 insertions(+), 43 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 1658a001..584a1a17 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,7 +12,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(ros_gz_bridge REQUIRED) find_package(std_msgs REQUIRED) @@ -138,7 +137,7 @@ add_library(ROSGzPlugin SHARED src/ros_gz.cpp) target_link_libraries(ROSGzPlugin PUBLIC ${GZ_TARGET_SIM}${GZ_SIM_VER}::core rclcpp::rclcpp - ros_gz_bridge::ros_gz_bridge_lib + ros_gz_bridge::ros_gz_bridge ) target_include_directories(ROSGzPlugin PUBLIC $ diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 7817af37..44c87f7f 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include #include +#include #include #include #include @@ -50,8 +50,11 @@ ROSGzPlugin::ROSGzPlugin() ////////////////////////////////////////////////// ROSGzPlugin::~ROSGzPlugin() { - this->dataPtr->exec->cancel(); - this->dataPtr->thread.join(); + if (this->dataPtr->exec) + { + this->dataPtr->exec->cancel(); + this->dataPtr->thread.join(); + } } ////////////////////////////////////////////////// @@ -72,16 +75,17 @@ void ROSGzPlugin::Configure( } // Sanity check: Make sure that the config file exists and it's a file. - std::filesystem::path configFile = _sdf->Get("config_file"); - if (!std::filesystem::is_regular_file(configFile)) { - std::cerr << "[" << configFile << "] is not a regular file. Plugin disabled" - << std::endl; + std::string filename = _sdf->Get("config_file"); + std::string path = gz::common::findFile(filename); + if (!gz::common::isFile(path)) { + std::cerr << "Unable to open YAML file [" << filename + << "], check your GAZEBO_RESOURCE_PATH settings." << std::endl; return; } // Create the bridge passing the parameters as rclcpp::NodeOptions(). this->dataPtr->bridge = std::make_shared( - rclcpp::NodeOptions().append_parameter_override("config_file", configFile)); + rclcpp::NodeOptions().append_parameter_override("config_file", path)); // Create the executor. this->dataPtr->exec = diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index 8e0f580a..632d2dd1 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -33,6 +33,13 @@ install( DESTINATION share/${PROJECT_NAME}/worlds ) + +install( + DIRECTORY + config/ + DESTINATION share +) + ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/ros_gz_sim_demos/worlds/ros_gz.sdf b/ros_gz_sim_demos/worlds/ros_gz.sdf index b1d66dbe..8a2360a5 100644 --- a/ros_gz_sim_demos/worlds/ros_gz.sdf +++ b/ros_gz_sim_demos/worlds/ros_gz.sdf @@ -6,6 +6,27 @@ + + 0.001 + 1.0 + + + + + + + + + + true 0 0 10 0 0 0 @@ -47,39 +68,10 @@ - - true - 0 0 0 0 0 0 - - package://ros_gz_sim_demos/models/vehicle - - - - - - - true - true - true - 1 - + + config/full.yaml + - - vehicle/odom - vehicle - - - - /home/caguero/ros_gz_ws/src/ros_gz/ros_gz_sim_demos/config/full.yaml - - - From 0ff8633e53ec3c3413ca0777b1b7e00bce041de5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 22 Feb 2024 21:30:10 +0100 Subject: [PATCH 20/25] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 584a1a17..cf741070 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(ros_gz_bridge REQUIRED) find_package(std_msgs REQUIRED) @@ -137,7 +138,7 @@ add_library(ROSGzPlugin SHARED src/ros_gz.cpp) target_link_libraries(ROSGzPlugin PUBLIC ${GZ_TARGET_SIM}${GZ_SIM_VER}::core rclcpp::rclcpp - ros_gz_bridge::ros_gz_bridge + ros_gz_bridge::ros_gz_bridge_lib ) target_include_directories(ROSGzPlugin PUBLIC $ @@ -145,7 +146,8 @@ target_include_directories(ROSGzPlugin PUBLIC ) install( TARGETS ROSGzPlugin - DESTINATION lib) + DESTINATION lib +) configure_file( launch/gz_sim.launch.py.in From 161f8ee18de1cc5f8212f2e50855e85d0d34b0f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 22 Feb 2024 21:44:29 +0100 Subject: [PATCH 21/25] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim_demos/CMakeLists.txt | 3 +-- ros_gz_sim_demos/worlds/ros_gz.sdf | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index 632d2dd1..d0626157 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -33,11 +33,10 @@ install( DESTINATION share/${PROJECT_NAME}/worlds ) - install( DIRECTORY config/ - DESTINATION share + DESTINATION share/${PROJECT_NAME}/config ) ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") diff --git a/ros_gz_sim_demos/worlds/ros_gz.sdf b/ros_gz_sim_demos/worlds/ros_gz.sdf index 8a2360a5..6144ca36 100644 --- a/ros_gz_sim_demos/worlds/ros_gz.sdf +++ b/ros_gz_sim_demos/worlds/ros_gz.sdf @@ -70,7 +70,7 @@ - config/full.yaml + ros_gz_sim_demos/config/full.yaml From ddf084c19d1d150e7529c7639b9a7b02dddf04ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Thu, 22 Feb 2024 22:34:44 +0100 Subject: [PATCH 22/25] Style MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/src/ros_gz.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ros_gz_sim/src/ros_gz.cpp b/ros_gz_sim/src/ros_gz.cpp index 44c87f7f..fc80089e 100644 --- a/ros_gz_sim/src/ros_gz.cpp +++ b/ros_gz_sim/src/ros_gz.cpp @@ -50,8 +50,7 @@ ROSGzPlugin::ROSGzPlugin() ////////////////////////////////////////////////// ROSGzPlugin::~ROSGzPlugin() { - if (this->dataPtr->exec) - { + if (this->dataPtr->exec) { this->dataPtr->exec->cancel(); this->dataPtr->thread.join(); } From 15cbccb7a8bc4726759318f7e24f4a214ec6ff62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 23 Feb 2024 17:45:41 +0100 Subject: [PATCH 23/25] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_bridge/CMakeLists.txt | 2 +- ros_gz_sim/CMakeLists.txt | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 023bd15b..7df2399b 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/ros_gz_bridge/CMakeLists.txt @@ -150,7 +150,7 @@ ament_target_dependencies(${bridge_lib} target_include_directories(${bridge_lib} PUBLIC - "$ + "$" "$" PRIVATE "$" diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index cf741070..114fe14a 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -12,7 +12,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(ros_gz_bridge REQUIRED) find_package(std_msgs REQUIRED) @@ -138,7 +137,7 @@ add_library(ROSGzPlugin SHARED src/ros_gz.cpp) target_link_libraries(ROSGzPlugin PUBLIC ${GZ_TARGET_SIM}${GZ_SIM_VER}::core rclcpp::rclcpp - ros_gz_bridge::ros_gz_bridge_lib + ros_gz_bridge::ros_gz_bridge ) target_include_directories(ROSGzPlugin PUBLIC $ From 93a08366780fb236f6b11b09194db86c391c9ccb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 23 Feb 2024 17:46:31 +0100 Subject: [PATCH 24/25] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_image/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/ros_gz_image/CMakeLists.txt b/ros_gz_image/CMakeLists.txt index 986e6282..545b0b27 100644 --- a/ros_gz_image/CMakeLists.txt +++ b/ros_gz_image/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED) find_package(image_transport REQUIRED) find_package(ros_gz_bridge REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) # TODO(CH3): Deprecated. Remove on tock. From f232b4b0cd2fee4e36795e00f027745b7a5fbbc8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 23 Feb 2024 17:48:09 +0100 Subject: [PATCH 25/25] Tweak MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 114fe14a..9057e003 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -140,8 +140,8 @@ target_link_libraries(ROSGzPlugin PUBLIC ros_gz_bridge::ros_gz_bridge ) target_include_directories(ROSGzPlugin PUBLIC - $ - $ + "$" + "$" ) install( TARGETS ROSGzPlugin