Fix for rosbag2_transport::Recorder failures due to unhandled exceptions

Signed-off-by: Michael Orlov <[email protected]>

Author: morlov-apexai

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -317,7 +317,13 @@ void RecorderImpl::event_publisher_thread_main()
       auto message = rosbag2_interfaces::msg::WriteSplitEvent();
       message.closed_file = bag_split_info_.closed_file;
       message.opened_file = bag_split_info_.opened_file;
-      split_event_pub_->publish(message);
+      try {
+        split_event_pub_->publish(message);
+      } catch (const std::exception & e) {
+        RCLCPP_ERROR_STREAM(
+          node->get_logger(),
+          "Failed to publish message on '/events/write_split' topic. \nError: " << e.what());
+      }
     }
 
     should_exit = event_publisher_thread_should_exit_;
@@ -365,8 +371,7 @@ bool RecorderImpl::is_paused()
 void RecorderImpl::topics_discovery()
 {
   while (rclcpp::ok() && stop_discovery_ == false) {
-    auto topics_to_subscribe =
-      get_requested_or_available_topics();
+    auto topics_to_subscribe = get_requested_or_available_topics();
     for (const auto & topic_and_type : topics_to_subscribe) {
       warn_if_new_qos_for_subscribed_topic(topic_and_type.first);
     }
@@ -386,7 +391,13 @@ void RecorderImpl::topics_discovery()
 std::unordered_map<std::string, std::string>
 RecorderImpl::get_requested_or_available_topics()
 {
-  auto all_topics_and_types = node->get_topic_names_and_types();
+  std::map<std::string, std::vector<std::string>> all_topics_and_types;
+  try {
+    all_topics_and_types = node->get_topic_names_and_types();
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      node->get_logger(), "Failed to get topic names and types from node \nError: " << e.what());
+  }
   return topic_filter_->filter_topics(all_topics_and_types);
 }
 
@@ -407,7 +418,16 @@ void RecorderImpl::subscribe_topics(
   const std::unordered_map<std::string, std::string> & topics_and_types)
 {
   for (const auto & topic_with_type : topics_and_types) {
-    auto endpoint_infos = node->get_publishers_info_by_topic(topic_with_type.first);
+    std::vector<rclcpp::TopicEndpointInfo> endpoint_infos;
+    try {
+      endpoint_infos = node->get_publishers_info_by_topic(topic_with_type.first);
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR_STREAM(
+        node->get_logger(), "Failed to get publishers info for '" << topic_with_type.first <<
+          "' topic. \nError: " << e.what());
+      continue;
+    }
+
     subscribe_topic(
       {
         topic_with_type.first,
@@ -427,13 +447,19 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic)
   writer_->create_topic(topic);
 
   Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)};
-  auto subscription = create_subscription(topic.name, topic.type, subscription_qos);
-  if (subscription) {
-    subscriptions_.insert({topic.name, subscription});
-    RCLCPP_INFO_STREAM(
-      node->get_logger(),
-      "Subscribed to topic '" << topic.name << "'");
-  } else {
+  try {
+    auto subscription = create_subscription(topic.name, topic.type, subscription_qos);
+    if (subscription) {
+      subscriptions_.insert({topic.name, subscription});
+      RCLCPP_INFO_STREAM(node->get_logger(), "Subscribed to topic '" << topic.name << "'");
+    } else {
+      writer_->remove_topic(topic);
+      subscriptions_.erase(topic.name);
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      node->get_logger(), "Failed to create subscription for '" << topic.name <<
+        "' topic. \nError: " << e.what());
     writer_->remove_topic(topic);
     subscriptions_.erase(topic.name);
   }
@@ -545,7 +571,16 @@ void RecorderImpl::warn_if_new_qos_for_subscribed_topic(const std::string & topi
   }
   const auto actual_qos = existing_subscription->second->get_actual_qos();
   const auto & used_profile = actual_qos.get_rmw_qos_profile();
-  auto publishers_info = node->get_publishers_info_by_topic(topic_name);
+
+  std::vector<rclcpp::TopicEndpointInfo> publishers_info;
+  try {
+    publishers_info = node->get_publishers_info_by_topic(topic_name);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR_STREAM(
+      node->get_logger(), "Failed to get publishers info for '" << topic_name <<
+        "' topic \nError: " << e.what());
+  }
+
   for (const auto & info : publishers_info) {
     auto new_profile = info.qos_profile().get_rmw_qos_profile();
     bool incompatible_reliability =

rosbag2_transport/src/rosbag2_transport/topic_filter.cpp

@@ -117,49 +117,56 @@ std::unordered_map<std::string, std::string> TopicFilter::filter_topics(
 bool TopicFilter::take_topic(
   const std::string & topic_name, const std::vector<std::string> & topic_types)
 {
-  if (!has_single_type(topic_name, topic_types)) {
-    return false;
-  }
+  try {
+    if (!has_single_type(topic_name, topic_types)) {
+      return false;
+    }
 
-  const std::string & topic_type = topic_types[0];
-  if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) {
-    return false;
-  }
+    const std::string & topic_type = topic_types[0];
+    if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) {
+      return false;
+    }
 
-  if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) {
-    RCUTILS_LOG_WARN_ONCE_NAMED(
-      ROSBAG2_TRANSPORT_PACKAGE_NAME,
-      "Hidden topics are not recorded. Enable them with --include-hidden-topics");
-    return false;
-  }
+    if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) {
+      RCUTILS_LOG_WARN_ONCE_NAMED(
+        ROSBAG2_TRANSPORT_PACKAGE_NAME,
+        "Hidden topics are not recorded. Enable them with --include-hidden-topics");
+      return false;
+    }
 
-  if (!record_options_.include_unpublished_topics && node_graph_ &&
-    topic_is_unpublished(topic_name, *node_graph_))
-  {
-    return false;
-  }
+    if (!record_options_.include_unpublished_topics && node_graph_ &&
+      topic_is_unpublished(topic_name, *node_graph_))
+    {
+      return false;
+    }
 
-  if (record_options_.ignore_leaf_topics && node_graph_ &&
-    is_leaf_topic(topic_name, *node_graph_))
-  {
-    return false;
-  }
+    if (record_options_.ignore_leaf_topics && node_graph_ &&
+      is_leaf_topic(topic_name, *node_graph_))
+    {
+      return false;
+    }
 
-  if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) {
-    return false;
-  }
+    if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) {
+      return false;
+    }
 
-  std::regex exclude_regex(record_options_.exclude);
-  if (!record_options_.exclude.empty() && std::regex_search(topic_name, exclude_regex)) {
-    return false;
-  }
+    std::regex exclude_regex(record_options_.exclude);
+    if (!record_options_.exclude.empty() && std::regex_search(topic_name, exclude_regex)) {
+      return false;
+    }
 
-  std::regex include_regex(record_options_.regex);
-  if (
-    !record_options_.all &&  // All takes precedence over regex
-    !record_options_.regex.empty() &&  // empty regex matches nothing, but should be ignored
-    !std::regex_search(topic_name, include_regex))
-  {
+    std::regex include_regex(record_options_.regex);
+    if (
+      !record_options_.all &&  // All takes precedence over regex
+      !record_options_.regex.empty() &&   // empty regex matches nothing, but should be ignored
+      !std::regex_search(topic_name, include_regex))
+    {
+      return false;
+    }
+  } catch (const std::exception & e) {
+    ROSBAG2_TRANSPORT_LOG_ERROR_STREAM(
+      "Failed to get information about topic '" << topic_name <<
+        "' in TopicFilter. \nError: " << e.what());
     return false;
   }