From cc222074fa1397d405bcd997e595ef99892e7383 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Fri, 29 Mar 2024 22:17:00 +0100
Subject: [PATCH] Tweaks
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
---
 .../include/ros_gz_bridge/ros_gz_bridge.hpp   |  2 +-
 ros_gz_bridge/src/bridge_config.cpp           | 23 +++++++++++++++++--
 ros_gz_bridge/src/ros_gz_bridge.cpp           |  3 +--
 ros_gz_bridge/test/bridge_config.cpp          |  5 ++--
 .../ros_gz_bridge_system.hpp                  |  0
 5 files changed, 25 insertions(+), 8 deletions(-)
 rename ros_gz_sim/{include/ros_gz_sim => src}/ros_gz_bridge_system.hpp (100%)

diff --git a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp
index 1e159507a..8e93a2717 100644
--- a/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp
+++ b/ros_gz_bridge/include/ros_gz_bridge/ros_gz_bridge.hpp
@@ -70,7 +70,7 @@ class RosGzBridge : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr heartbeat_timer_;
 
   /// \brief Whether the config_file parameter was parsed or not.
-  bool config_file_parsed_;
+  bool config_file_parsed_{false};
 };
 }  // namespace ros_gz_bridge
 
diff --git a/ros_gz_bridge/src/bridge_config.cpp b/ros_gz_bridge/src/bridge_config.cpp
index ac1fbb46f..d83cc7a9b 100644
--- a/ros_gz_bridge/src/bridge_config.cpp
+++ b/ros_gz_bridge/src/bridge_config.cpp
@@ -174,13 +174,32 @@ std::vector<BridgeConfig> readFromYaml(std::istream & in)
 std::vector<BridgeConfig> readFromYamlFile(const std::string & filename)
 {
   std::vector<BridgeConfig> ret;
-  resource_retriever::Retriever r;
+  std::ifstream in(filename);
+  resource_retriever::Retriever retriever;
   resource_retriever::MemoryResource res;
 
   auto logger = rclcpp::get_logger("readFromYamlFile");
 
+  if (in.is_open()) {
+    // 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) {
+      RCLCPP_ERROR(
+        logger,
+        "Could not parse config: file empty [%s]", filename.c_str());
+      return ret;
+    }
+
+    in.seekg(0, std::ios::beg);
+    return readFromYaml(in);
+  }
+
   try {
-    res = r.get(filename);
+    res = retriever.get(filename);
   } catch (const resource_retriever::Exception & exc) {
     RCLCPP_ERROR(
       logger,
diff --git a/ros_gz_bridge/src/ros_gz_bridge.cpp b/ros_gz_bridge/src/ros_gz_bridge.cpp
index 3b15bccac..2f712e87d 100644
--- a/ros_gz_bridge/src/ros_gz_bridge.cpp
+++ b/ros_gz_bridge/src/ros_gz_bridge.cpp
@@ -24,8 +24,7 @@ namespace ros_gz_bridge
 {
 
 RosGzBridge::RosGzBridge(const rclcpp::NodeOptions & options)
-: rclcpp::Node("ros_gz_bridge", options),
-  config_file_parsed_(false)
+: rclcpp::Node("ros_gz_bridge", options)
 {
   gz_node_ = std::make_shared<gz::transport::Node>();
 
diff --git a/ros_gz_bridge/test/bridge_config.cpp b/ros_gz_bridge/test/bridge_config.cpp
index ac192abfe..9d261be5f 100644
--- a/ros_gz_bridge/test/bridge_config.cpp
+++ b/ros_gz_bridge/test/bridge_config.cpp
@@ -70,9 +70,8 @@ class BridgeConfig : public ::testing::Test
 
 TEST_F(BridgeConfig, Minimum)
 {
-  auto results = ros_gz_bridge::readFromYamlFile(
-    "package://ros_gz_bridge/test/config/minimum.yaml");
-  EXPECT_EQ(4u, results.size());
+  auto results = ros_gz_bridge::readFromYamlFile("test/config/minimum.yaml");
+  ASSERT_EQ(4u, results.size());
 
   {
     auto config = results[0];
diff --git a/ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp b/ros_gz_sim/src/ros_gz_bridge_system.hpp
similarity index 100%
rename from ros_gz_sim/include/ros_gz_sim/ros_gz_bridge_system.hpp
rename to ros_gz_sim/src/ros_gz_bridge_system.hpp