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 readFromYaml(std::istream & in) std::vector readFromYamlFile(const std::string & filename) { std::vector 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(); 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