diff --git a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp index 3b7fe2a..72bab6f 100644 --- a/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp +++ b/ros1_foxglove_bridge/src/ros1_foxglove_bridge_nodelet.cpp @@ -48,6 +48,7 @@ constexpr uint32_t SUBSCRIPTION_QUEUE_LENGTH = 10; constexpr double MIN_UPDATE_PERIOD_MS = 100.0; constexpr uint32_t PUBLICATION_QUEUE_LENGTH = 10; constexpr int DEFAULT_SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS = 250; +constexpr int MAX_INVALID_PARAMS_TRACKED = 1000; using ConnectionHandle = websocketpp::connection_hdl; using TopicAndDatatype = std::pair; @@ -656,6 +657,7 @@ class FoxgloveBridge : public nodelet::Nodelet { bool success = true; std::vector params; + std::vector invalidParams; for (const auto& paramName : parameterNames) { if (!isWhitelisted(paramName, _paramWhitelistPatterns)) { if (allParametersRequested) { @@ -665,6 +667,9 @@ class FoxgloveBridge : public nodelet::Nodelet { success = false; } } + if (_invalidParams.find(paramName) != _invalidParams.end()) { + continue; + } try { XmlRpc::XmlRpcValue value; @@ -672,12 +677,15 @@ class FoxgloveBridge : public nodelet::Nodelet { params.push_back(fromRosParam(paramName, value)); } catch (const std::exception& ex) { ROS_ERROR("Invalid parameter '%s': %s", paramName.c_str(), ex.what()); + invalidParams.push_back(paramName); success = false; } catch (const XmlRpc::XmlRpcException& ex) { ROS_ERROR("Invalid parameter '%s': %s", paramName.c_str(), ex.getMessage().c_str()); + invalidParams.push_back(paramName); success = false; } catch (...) { ROS_ERROR("Invalid parameter '%s'", paramName.c_str()); + invalidParams.push_back(paramName); success = false; } } @@ -685,7 +693,24 @@ class FoxgloveBridge : public nodelet::Nodelet { _server->publishParameterValues(hdl, params, requestId); if (!success) { - throw std::runtime_error("Failed to retrieve one or multiple parameters"); + for (std::string& param : invalidParams) { + if (_invalidParams.size() < MAX_INVALID_PARAMS_TRACKED) { + _invalidParams.insert(param); + } + } + + if (!invalidParams.empty()) { + std::string errorMsg = "Failed to retrieve the following parameters: "; + for (size_t i = 0; i < invalidParams.size(); i++) { + errorMsg += invalidParams[i]; + if (i < invalidParams.size() - 1) { + errorMsg += ", "; + } + } + throw std::runtime_error(errorMsg); + } else { + throw std::runtime_error("Failed to retrieve one or multiple parameters"); + } } } @@ -922,6 +947,7 @@ class FoxgloveBridge : public nodelet::Nodelet { int _serviceRetrievalTimeoutMs = DEFAULT_SERVICE_TYPE_RETRIEVAL_TIMEOUT_MS; std::atomic _subscribeGraphUpdates = false; std::unique_ptr _fetchAssetQueue; + std::unordered_set _invalidParams; }; } // namespace foxglove_bridge