Add more exception handling (#191)

### Public-Facing Changes - Add more exception handling ### Description Adds more exception handling in places where it is unlikely but not impossible that exceptions are being thrown
foxglove · Mar 9, 2023 · 9a2ccaa · 9a2ccaa
1 parent 51fd89a
commit 9a2ccaa
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 37 deletions.
diff --git a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp
@@ -449,7 +449,13 @@ class FoxgloveBridge : public nodelet::Nodelet {
         _server->remoteEndpointString(clientHandle).c_str(), channelId, clientPublications.size());
       return;
     }
-    channelPublicationIt->second.publish(msg);
+
+    try {
+      channelPublicationIt->second.publish(msg);
+    } catch (const std::exception& ex) {
+      ROS_ERROR("Failed to publish message on topic '%s'",
+                channelPublicationIt->second.getTopic().c_str());
+    }
   }
 
   void updateAdvertisedTopicsAndServices(const ros::TimerEvent&) {
@@ -682,7 +688,10 @@ class FoxgloveBridge : public nodelet::Nodelet {
                      const std::optional<std::string>& requestId, ConnectionHandle hdl) {
     std::vector<std::string> parameterNames = parameters;
     if (parameterNames.empty()) {
-      getMTNodeHandle().getParamNames(parameterNames);
+      if (!getMTNodeHandle().getParamNames(parameterNames)) {
+        ROS_ERROR("Failed to retrieve parameter names");
+        return;
+      }
     }
 
     std::vector<foxglove::Parameter> params;
@@ -715,27 +724,31 @@ class FoxgloveBridge : public nodelet::Nodelet {
         continue;
       }
 
-      const auto paramType = param.getType();
-      if (paramType == ParameterType::PARAMETER_BOOL) {
-        nh.setParam(paramName, param.getValue<bool>());
-      } else if (paramType == ParameterType::PARAMETER_INTEGER) {
-        nh.setParam(paramName, static_cast<int>(param.getValue<int64_t>()));
-      } else if (paramType == ParameterType::PARAMETER_DOUBLE) {
-        nh.setParam(paramName, param.getValue<double>());
-      } else if (paramType == ParameterType::PARAMETER_STRING) {
-        nh.setParam(paramName, param.getValue<std::string>());
-      } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) {
-        nh.setParam(paramName, param.getValue<std::vector<bool>>());
-      } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) {
-        const auto int64Vec = param.getValue<std::vector<int64_t>>();
-        std::vector<int> intVec(int64Vec.begin(), int64Vec.end());
-        nh.setParam(paramName, intVec);
-      } else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) {
-        nh.setParam(paramName, param.getValue<std::vector<double>>());
-      } else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) {
-        nh.setParam(paramName, param.getValue<std::vector<std::string>>());
-      } else if (paramType == ParameterType::PARAMETER_NOT_SET) {
-        nh.deleteParam(paramName);
+      try {
+        const auto paramType = param.getType();
+        if (paramType == ParameterType::PARAMETER_BOOL) {
+          nh.setParam(paramName, param.getValue<bool>());
+        } else if (paramType == ParameterType::PARAMETER_INTEGER) {
+          nh.setParam(paramName, static_cast<int>(param.getValue<int64_t>()));
+        } else if (paramType == ParameterType::PARAMETER_DOUBLE) {
+          nh.setParam(paramName, param.getValue<double>());
+        } else if (paramType == ParameterType::PARAMETER_STRING) {
+          nh.setParam(paramName, param.getValue<std::string>());
+        } else if (paramType == ParameterType::PARAMETER_BOOL_ARRAY) {
+          nh.setParam(paramName, param.getValue<std::vector<bool>>());
+        } else if (paramType == ParameterType::PARAMETER_INTEGER_ARRAY) {
+          const auto int64Vec = param.getValue<std::vector<int64_t>>();
+          std::vector<int> intVec(int64Vec.begin(), int64Vec.end());
+          nh.setParam(paramName, intVec);
+        } else if (paramType == ParameterType::PARAMETER_DOUBLE_ARRAY) {
+          nh.setParam(paramName, param.getValue<std::vector<double>>());
+        } else if (paramType == ParameterType::PARAMETER_STRING_ARRAY) {
+          nh.setParam(paramName, param.getValue<std::vector<std::string>>());
+        } else if (paramType == ParameterType::PARAMETER_NOT_SET) {
+          nh.deleteParam(paramName);
+        }
+      } catch (const std::exception& ex) {
+        ROS_ERROR("Failed to set parameter '%s': %s", paramName.c_str(), ex.what());
       }
     }
 
@@ -784,9 +797,9 @@ class FoxgloveBridge : public nodelet::Nodelet {
       return;
     }
 
-    const std::string paramName = ros::names::clean(params[1]);
-    const XmlRpc::XmlRpcValue paramValue = params[2];
     try {
+      const std::string paramName = ros::names::clean(params[1]);
+      const XmlRpc::XmlRpcValue paramValue = params[2];
       const auto param = fromRosParam(paramName, paramValue);
       _server->updateParameterValues({param});
     } catch (const std::exception& ex) {

diff --git a/ros1_foxglove_bridge/src/service_utils.cpp b/ros1_foxglove_bridge/src/service_utils.cpp
@@ -17,6 +17,9 @@ std::future<std::string> retrieveServiceType(const std::string& serviceName) {
       std::string serviceType;
       if (header.getValue("type", serviceType)) {
         promise->set_value(serviceType);
+      } else {
+        promise->set_exception(std::make_exception_ptr(
+          std::runtime_error("Key 'type' not found in service connection header")));
       }
       return true;
     });

diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/callback_queue.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/callback_queue.hpp
@@ -8,6 +8,8 @@
 #include <thread>
 #include <vector>
 
+#include <rclcpp/logging.hpp>
+
 namespace foxglove_bridge {
 
 class CallbackQueue {
@@ -53,7 +55,13 @@ class CallbackQueue {
         std::function<void(void)> cb = _callbackQueue.front();
         _callbackQueue.pop_front();
         lock.unlock();
-        cb();
+        try {
+          cb();
+        } catch (const std::exception& ex) {
+          // Should never get here if we catch all exceptions in the callbacks.
+          RCLCPP_ERROR(rclcpp::get_logger("foxglove_bridge"),
+                       "Caught unhandled exception in calback_queue: %s", ex.what());
+        }
       }
     }
   }

diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp
@@ -156,18 +156,22 @@ class FoxgloveBridge : public rclcpp::Node {
 
     auto graphEvent = this->get_graph_event();
     while (rclcpp::ok()) {
-      this->wait_for_graph_change(graphEvent, 200ms);
-      bool triggered = graphEvent->check_and_clear();
-      if (triggered) {
-        RCLCPP_DEBUG(this->get_logger(), "rosgraph change detected");
-        const auto topicNamesAndTypes = get_topic_names_and_types();
-        updateAdvertisedTopics(topicNamesAndTypes);
-        updateAdvertisedServices();
-        if (_subscribeGraphUpdates) {
-          updateConnectionGraph(topicNamesAndTypes);
+      try {
+        this->wait_for_graph_change(graphEvent, 200ms);
+        bool triggered = graphEvent->check_and_clear();
+        if (triggered) {
+          RCLCPP_DEBUG(this->get_logger(), "rosgraph change detected");
+          const auto topicNamesAndTypes = get_topic_names_and_types();
+          updateAdvertisedTopics(topicNamesAndTypes);
+          updateAdvertisedServices();
+          if (_subscribeGraphUpdates) {
+            updateConnectionGraph(topicNamesAndTypes);
+          }
+          // Graph changes tend to come in batches, so wait a bit before checking again
+          std::this_thread::sleep_for(500ms);
         }
-        // Graph changes tend to come in batches, so wait a bit before checking again
-        std::this_thread::sleep_for(500ms);
+      } catch (const std::exception& ex) {
+        RCLCPP_ERROR(this->get_logger(), "Exception thrown in rosgraphPollThread: %s", ex.what());
       }
     }