Use resource_retriever in the bridge (#515)

Signed-off-by: Carlos Agüero <[email protected]>
Signed-off-by: Carlos Agüero <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>

ros_gz_bridge/CMakeLists.txt

@@ -13,6 +13,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(resource_retriever REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
 if(NOT DEFINED ENV{GZ_VERSION})
@@ -120,6 +121,7 @@ add_library(${bridge_lib}
 target_link_libraries(${bridge_lib}
   ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
   ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
+  resource_retriever::resource_retriever
 )
 
 ament_target_dependencies(${bridge_lib}
@@ -309,6 +311,11 @@ if(BUILD_TESTING)
     DESTINATION lib/${PROJECT_NAME}
   )
 
+  install(DIRECTORY
+    test/config
+    DESTINATION share/${PROJECT_NAME}/test/
+  )
+
   add_launch_test(test/launch/test_ros_subscriber.launch.py
     TIMEOUT 200
     ARGS "gz_msgs_ver:=${GZ_MSGS_VERSION_FULL}"
@@ -341,6 +348,7 @@ ament_export_targets(export_${PROJECT_NAME})
 # specific order: dependents before dependencies
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rclcpp_components)
+ament_export_dependencies(resource_retriever)
 ament_export_dependencies(${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER})
 ament_export_dependencies(${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER})
 ament_export_dependencies(yaml_cpp_vendor)

ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp

@@ -68,6 +68,9 @@ class RosGzBridge : public rclcpp::Node
 
   /// \brief Timer to control periodic callback
   rclcpp::TimerBase::SharedPtr heartbeat_timer_;
+
+  /// \brief Whether the config_file parameter was parsed or not.
+  bool config_file_parsed_;
 };
 }  // namespace ros_gz_bridge
 

ros_gz_bridge/package.xml

@@ -22,6 +22,7 @@
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>resource_retriever</depend>
   <depend>ros_gz_interfaces</depend>
   <depend>rosgraph_msgs</depend>
   <depend>sensor_msgs</depend>

ros_gz_bridge/src/bridge_config.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 
 #include <rclcpp/rclcpp.hpp>
+#include <resource_retriever/retriever.hpp>
 #include <ros_gz_bridge/bridge_config.hpp>
 
 namespace ros_gz_bridge
@@ -173,31 +174,30 @@ std::vector<BridgeConfig> readFromYaml(std::istream & in)
 std::vector<BridgeConfig> readFromYamlFile(const std::string & filename)
 {
   std::vector<BridgeConfig> ret;
-  std::ifstream in(filename);
+  resource_retriever::Retriever r;
+  resource_retriever::MemoryResource res;
 
   auto logger = rclcpp::get_logger("readFromYamlFile");
-  if (!in.is_open()) {
+
+  try {
+    res = r.get(filename);
+  } catch (const resource_retriever::Exception & exc) {
     RCLCPP_ERROR(
       logger,
       "Could not parse config: failed to open file [%s]", filename.c_str());
     return ret;
   }
 
-  // Compute file size to warn on empty configuration
-  const auto fbegin = in.tellg();
-  in.seekg(0, std::ios::end);
-  const auto fend = in.tellg();
-  const auto fsize = fend - fbegin;
-
-  if (fsize == 0) {
+  // Sanity check: Avoid empty configuration.
+  if (res.size == 0) {
     RCLCPP_ERROR(
       logger,
       "Could not parse config: file empty [%s]", filename.c_str());
     return ret;
   }
 
-  in.seekg(0, std::ios::beg);
-  return readFromYaml(in);
+  std::string str(reinterpret_cast<char *>(res.data.get()), res.size);
+  return readFromYamlString(str);
 }
 
 std::vector<BridgeConfig> readFromYamlString(const std::string & data)

ros_gz_bridge/src/ros_gz_bridge.cpp

@@ -12,19 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <ros_gz_bridge/ros_gz_bridge.hpp>
-
 #include <memory>
 #include <string>
 
+#include <ros_gz_bridge/ros_gz_bridge.hpp>
+
 #include "bridge_handle_ros_to_gz.hpp"
 #include "bridge_handle_gz_to_ros.hpp"
 
 namespace ros_gz_bridge
 {
 
 RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options)
-: rclcpp::Node("ros_gz_bridge", options)
+: rclcpp::Node("ros_gz_bridge", options),
+  config_file_parsed_(false)
 {
   gz_node_ = std::make_shared<gz::transport::Node>();
 
@@ -43,7 +44,8 @@ void RosGzBridge::spin()
   if (handles_.empty()) {
     std::string config_file;
     this->get_parameter("config_file", config_file);
-    if (!config_file.empty()) {
+    if (!config_file.empty() && !config_file_parsed_) {
+      config_file_parsed_ = true;
       auto entries = readFromYamlFile(config_file);
       for (const auto & entry : entries) {
         this->add_bridge(entry);

ros_gz_bridge/test/bridge_config.cpp

@@ -70,15 +70,16 @@ class BridgeConfig : public ::testing::Test
 
 TEST_F(BridgeConfig, Minimum)
 {
-  auto results = ros_gz_bridge::readFromYamlFile("test/config/minimum.yaml");
+  auto results = ros_gz_bridge::readFromYamlFile(
+    "package://ros_gz_bridge/test/config/minimum.yaml");
   EXPECT_EQ(4u, results.size());
 
   {
     auto config = results[0];
     EXPECT_EQ("chatter", config.ros_topic_name);
     EXPECT_EQ("chatter", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy);
@@ -88,7 +89,7 @@ TEST_F(BridgeConfig, Minimum)
     EXPECT_EQ("chatter_ros", config.ros_topic_name);
     EXPECT_EQ("chatter_ros", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy);
@@ -98,7 +99,7 @@ TEST_F(BridgeConfig, Minimum)
     EXPECT_EQ("chatter_gz", config.ros_topic_name);
     EXPECT_EQ("chatter_gz", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy);
@@ -108,7 +109,7 @@ TEST_F(BridgeConfig, Minimum)
     EXPECT_EQ("chatter_both_ros", config.ros_topic_name);
     EXPECT_EQ("chatter_both_gz", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(ros_gz_bridge::kDefaultPublisherQueue, config.publisher_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultSubscriberQueue, config.subscriber_queue_size);
     EXPECT_EQ(ros_gz_bridge::kDefaultLazy, config.is_lazy);
@@ -117,15 +118,16 @@ TEST_F(BridgeConfig, Minimum)
 
 TEST_F(BridgeConfig, FullGz)
 {
-  auto results = ros_gz_bridge::readFromYamlFile("test/config/full.yaml");
+  auto results = ros_gz_bridge::readFromYamlFile(
+    "package://ros_gz_bridge/test/config/full.yaml");
   EXPECT_EQ(2u, results.size());
 
   {
     auto config = results[0];
     EXPECT_EQ("ros_chatter", config.ros_topic_name);
     EXPECT_EQ("gz_chatter", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(6u, config.publisher_queue_size);
     EXPECT_EQ(5u, config.subscriber_queue_size);
     EXPECT_EQ(true, config.is_lazy);
@@ -137,7 +139,7 @@ TEST_F(BridgeConfig, FullGz)
     EXPECT_EQ("ros_chatter", config.ros_topic_name);
     EXPECT_EQ("gz_chatter", config.gz_topic_name);
     EXPECT_EQ("std_msgs/msg/String", config.ros_type_name);
-    EXPECT_EQ("ignition.msgs.StringMsg", config.gz_type_name);
+    EXPECT_EQ("gz.msgs.StringMsg", config.gz_type_name);
     EXPECT_EQ(20u, config.publisher_queue_size);
     EXPECT_EQ(10u, config.subscriber_queue_size);
     EXPECT_EQ(false, config.is_lazy);
@@ -195,7 +197,7 @@ TEST_F(BridgeConfig, ParseDirection)
       R"(
   - topic_name: foo
     ros_type_name: std_msgs/msg/String
-    gz_type_name: ignition.msgs.StringMsg)";
+    gz_type_name: gz.msgs.StringMsg)";
 
     auto results = ros_gz_bridge::readFromYamlString(yaml);
     EXPECT_EQ(ros_gz_bridge::BridgeDirection::BIDIRECTIONAL, results[0].direction);
@@ -206,7 +208,7 @@ TEST_F(BridgeConfig, ParseDirection)
       R"(
   - topic_name: foo
     ros_type_name: std_msgs/msg/String
-    gz_type_name: ignition.msgs.StringMsg
+    gz_type_name: gz.msgs.StringMsg
     direction: BIDIRECTIONAL
     )";
 
@@ -219,7 +221,7 @@ TEST_F(BridgeConfig, ParseDirection)
       R"(
   - topic_name: foo
     ros_type_name: std_msgs/msg/String
-    gz_type_name: ignition.msgs.StringMsg
+    gz_type_name: gz.msgs.StringMsg
     direction: ROS_TO_GZ
     )";
 
@@ -232,7 +234,7 @@ TEST_F(BridgeConfig, ParseDirection)
       R"(
   - topic_name: foo
     ros_type_name: std_msgs/msg/String
-    gz_type_name: ignition.msgs.StringMsg
+    gz_type_name: gz.msgs.StringMsg
     direction: GZ_TO_ROS
     )";
 
@@ -246,7 +248,7 @@ TEST_F(BridgeConfig, ParseDirection)
       R"(
   - topic_name: foo
     ros_type_name: std_msgs/msg/String
-    gz_type_name: ignition.msgs.StringMsg
+    gz_type_name: gz.msgs.StringMsg
     direction: foobar
     )";
 
@@ -267,7 +269,8 @@ TEST_F(BridgeConfig, InvalidFileDoesntExist)
 
 TEST_F(BridgeConfig, InvalidTopLevel)
 {
-  auto results = ros_gz_bridge::readFromYamlFile("test/config/invalid.yaml");
+  auto results = ros_gz_bridge::readFromYamlFile(
+    "package://ros_gz_bridge/test/config/invalid.yaml");
   EXPECT_EQ(0u, results.size());
   EXPECT_EQ(
     "Could not parse config: top level must be a YAML sequence",
@@ -276,9 +279,11 @@ TEST_F(BridgeConfig, InvalidTopLevel)
 
 TEST_F(BridgeConfig, EmptyYAML)
 {
-  auto results = ros_gz_bridge::readFromYamlFile("test/config/empty.yaml");
+  auto results = ros_gz_bridge::readFromYamlFile(
+    "package://ros_gz_bridge/test/config/empty.yaml");
   EXPECT_EQ(0u, results.size());
   EXPECT_EQ(
-    "Could not parse config: file empty [test/config/empty.yaml]",
+    "Could not parse config: file empty ["
+    "package://ros_gz_bridge/test/config/empty.yaml]",
     g_last_log_event.message);
 }

ros_gz_bridge/test/config/full.yaml

@@ -2,7 +2,7 @@
 - ros_topic_name: "ros_chatter"
   gz_topic_name: "gz_chatter"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
   subscriber_queue: 5
   publisher_queue: 6
   lazy: true
@@ -11,7 +11,7 @@
 - ros_topic_name: "ros_chatter"
   gz_topic_name: "gz_chatter"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
   subscriber_queue: 10
   publisher_queue: 20
   lazy: false

ros_gz_bridge/test/config/invalid.yaml

@@ -1,7 +1,7 @@
 ros_topic_name: "ros_chatter"
 gz_topic_name: "gz_chatter"
 ros_type_name: "std_msgs/msg/String"
-gz_type_name: "ignition.msgs.StringMsg"
+gz_type_name: "gz.msgs.StringMsg"
 subscriber_queue: 5
 publisher_queue: 6
 lazy: true

ros_gz_bridge/test/config/minimum.yaml

@@ -1,20 +1,20 @@
 # Set just topic name, applies to both
 - topic_name: "chatter"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set just ROS topic name, applies to both
 - ros_topic_name: "chatter_ros"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set just GZ topic name, applies to both
 - gz_topic_name: "chatter_gz"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"
 
 # Set each topic name explicitly
 - ros_topic_name: "chatter_both_ros"
   gz_topic_name: "chatter_both_gz"
   ros_type_name: "std_msgs/msg/String"
-  gz_type_name: "ignition.msgs.StringMsg"
+  gz_type_name: "gz.msgs.StringMsg"

ros_gz_sim/src/ros_gz_bridge_system.cpp

@@ -73,18 +73,11 @@ void ROSGzBridgeSystem::Configure(
     return;
   }
 
-  // Sanity check: Make sure that the config file exists and it's a file.
   std::string filename = _sdf->Get<std::string>("config_file");
-  std::string path = gz::common::findFile(filename);
-  if (!gz::common::isFile(path)) {
-    std::cerr << "Unable to open YAML file [" << filename
-              << "], check your GZ_SIM_RESOURCE_PATH settings." << std::endl;
-    return;
-  }
 
   // Create the bridge passing the parameters as rclcpp::NodeOptions().
   this->dataPtr->bridge = std::make_shared<ros_gz_bridge::RosGzBridge>(
-    rclcpp::NodeOptions().append_parameter_override("config_file", path));
+    rclcpp::NodeOptions().append_parameter_override("config_file", filename));
 
   // Create the executor.
   this->dataPtr->exec =

ros_gz_sim_demos/CMakeLists.txt

@@ -33,12 +33,6 @@ install(
   DESTINATION share/${PROJECT_NAME}/worlds
 )
 
-install(
-  DIRECTORY
-    config/
-  DESTINATION share/${PROJECT_NAME}/config
-)
-
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
 
 ament_package()

ros_gz_sim_demos/worlds/ros_gz.sdf

@@ -70,7 +70,7 @@
 
     <plugin name="ros_gz_sim::ROSGzBridge"
             filename="ros-gz-bridge-system">
-      <config_file>package://ros_gz_sim_demos/config/full.yaml</config_file>
+      <config_file>package://ros_gz_bridge/test/config/full.yaml</config_file>
     </plugin>
 
   </world>