diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h index 1d949fed3..6f9638629 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h @@ -35,6 +35,7 @@ #define FUSE_CONSTRAINTS_NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H #include +#include #include #include @@ -105,7 +106,7 @@ class NormalPriorOrientation3DEulerCostFunctor for (size_t i = 0; i < axes_.size(); ++i) { - T angle; + T angle = T(0); switch (axes_[i]) { case Euler::ROLL: @@ -125,7 +126,7 @@ class NormalPriorOrientation3DEulerCostFunctor } default: { - throw std::runtime_error("The provided axis specified is unknown. " + fuse_core::ErrorHandler::getHandler().runtimeError("The provided axis specified is unknown. " "I should probably be more informative here"); } } diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 22d9a8950..ff346f5aa 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -147,7 +148,7 @@ UuidOrdering computeEliminationOrder( variable_groups.data()); if (!success) { - throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call CCOLAMD to generate the elimination order."); } // Extract the elimination order from CCOLAMD. @@ -321,7 +322,8 @@ LinearTerm linearize( } if (!success) { - throw std::runtime_error("Error in evaluating the cost function. There are two possible reasons. " + fuse_core::ErrorHandler::getHandler().runtimeError("Error in evaluating the cost function. " + "There are two possible reasons. " "Either the CostFunction did not evaluate and fill all residual and jacobians " "that were requested or there was a non-finite value (nan/infinite) generated " "during the jacobian computation."); diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index ee0d6d863..17c4cf966 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -286,6 +286,28 @@ if(CATKIN_ENABLE_TESTING) CXX_STANDARD_REQUIRED YES ) + # Error handler tests + catkin_add_gtest(test_error_handler + test/test_error_handler.cpp + ) + add_dependencies(test_error_handler + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_error_handler + PRIVATE + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + target_link_libraries(test_error_handler + ${catkin_LIBRARIES} + ) + set_target_properties(test_error_handler + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + # Local Parameterization tests catkin_add_gtest(test_local_parameterization test/test_local_parameterization.cpp diff --git a/fuse_core/include/fuse_core/error_handler.h b/fuse_core/include/fuse_core/error_handler.h new file mode 100644 index 000000000..5a8e8e79b --- /dev/null +++ b/fuse_core/include/fuse_core/error_handler.h @@ -0,0 +1,273 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FUSE_CORE_ERROR_HANDLER_H +#define FUSE_CORE_ERROR_HANDLER_H + +#include "ros/ros.h" +#include +#include +#include + +/** + * @brief A class that handles errors that can occur in fuse. + * + * By default, fuse optimizers handle any errors that come up by throwing a relevant exception. However, + * while this is generically exceptable, in practical systems it's sometimes not sufficient to just + * throw exceptions and hope that the rest of the system goes on while fuse resets and fixes itself. + * + * This class exists to allow for handling generic classes of errors using whatever callback the user sees fit to use. + * The default behavior is to throw the relevant exception. Any function that has void return and accepts a string + * can be used for this, however. + * + * The error handler also allows for different error handlers to be used in different contexts. It assumes that unless + * otherwise specified, all error handling is done generically. However, errors in particular contexts can be handled + * with unique callbacks as needed. + * + * It's recommended that you register any needed error non-default callbacks during Optimizer startup. + */ + +namespace fuse_core +{ + +/** + * @brief Used to provide different error handlers for a given context. + */ +enum class ErrorContext: int +{ + GENERIC, + CORE, + CONSTRAINT, + GRAPH, + LOSS, + MODEL, + OPTIMIZER, + PUBLISHER, + VARIABLE, + VIZ +}; + +class ErrorHandler +{ + using ErrorCb = std::function; + public: + /** + * @brief Gets a reference to the error handler. + * + * @param[out] handler - reference to the error handler + */ + static ErrorHandler& getHandler() + { + static ErrorHandler handler {}; + return handler; + } + + /** + * @brief Use whatever callback is registered to handle an invalid argument error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void invalidArgument(const std::string& info, ErrorContext context = ErrorContext::GENERIC) + { + try + { + auto cb = invalid_argument_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call invalidArgument Error handling, no callback found for error " + info); + } + } + /** + * @brief Use whatever callback is registered to handle a runtime error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void runtimeError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) + { + try + { + auto cb = runtime_error_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call runtimeError Error handling, no callback found for error " + info); + } + } + + /** + * @brief Use whatever callback is registered to handle an out of range error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void outOfRangeError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) + { + try + { + auto cb = out_of_range_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call outOfRangeError handling, no callback found for error " + info); + } + } + + /** + * @brief Use whatever callback is registered to handle a logic error for a given context. + * + * @param[in] info A string containing information about the error. + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + * @throws std::out_of_range if no callback has been registered for the provided context + */ + void logicError(const std::string& info, ErrorContext context = ErrorContext::GENERIC) + { + try + { + auto cb = logic_error_cbs_.at(context); + cb(info); + } + catch(const std::out_of_range& e) + { + throw std::out_of_range("Failed to call logicError handling, no callback found for error " + info); + } + } + + /** + * @brief Register a callback to be used for invalid argument errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerinvalidArgumentErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) + { + invalid_argument_cbs_[context] = cb; + } + + /** + * @brief Register a callback to be used for runtime errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerRuntimeErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) + { + runtime_error_cbs_[context] = cb; + } + + /** + * @brief Register a callback to be used for out of range errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerOutOfRangeErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) + { + out_of_range_cbs_[context] = cb; + } + + /** + * @brief Register a callback to be used for logic errors in a particular context + * + * @param[in] cb The callback to be registered + * @param[in] context The Error Context, which dictates what callback is used. Defaults to ErrorContext::Generic + */ + void registerLogicErrorCb(ErrorCb cb, ErrorContext context = ErrorContext::GENERIC) + { + logic_error_cbs_[context] = cb; + } + + // Should be used only for teardown and setup during unit tests + void reset() + { + invalid_argument_cbs_.clear(); + runtime_error_cbs_.clear(); + out_of_range_cbs_.clear(); + logic_error_cbs_.clear(); + + invalid_argument_cbs_.insert(std::make_pair(ErrorContext::GENERIC, invalidArgumentCallback)); + runtime_error_cbs_.insert(std::make_pair(ErrorContext::GENERIC, runtimeErrorCallback)); + out_of_range_cbs_.insert(std::make_pair(ErrorContext::GENERIC, outOfRangeErrorCallback)); + logic_error_cbs_.insert(std::make_pair(ErrorContext::GENERIC, logicErrorCallback)); + } + + private: + ErrorHandler() + { + invalid_argument_cbs_[ErrorContext::GENERIC] = invalidArgumentCallback; + runtime_error_cbs_[ErrorContext::GENERIC] = runtimeErrorCallback; + out_of_range_cbs_[ErrorContext::GENERIC] = outOfRangeErrorCallback; + logic_error_cbs_[ErrorContext::GENERIC] = logicErrorCallback; + } + + ~ErrorHandler() {} + + std::unordered_map invalid_argument_cbs_; + std::unordered_map runtime_error_cbs_; + std::unordered_map out_of_range_cbs_; + std::unordered_map logic_error_cbs_; + + /* + * Default error handling behavior + */ + static void invalidArgumentCallback(std::string info) + { + throw std::invalid_argument(info); + } + static void logicErrorCallback(std::string info) + { + throw std::logic_error(info); + } + static void runtimeErrorCallback(std::string info) + { + throw std::runtime_error(info); + } + static void outOfRangeErrorCallback(std::string info) + { + throw std::out_of_range(info); + } +}; + +} // namespace fuse_core + + +#endif // FUSE_CORE_ERROR_HANDLER_H diff --git a/fuse_core/include/fuse_core/message_buffer_impl.h b/fuse_core/include/fuse_core/message_buffer_impl.h index d83cd668d..30616ffb7 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.h +++ b/fuse_core/include/fuse_core/message_buffer_impl.h @@ -34,6 +34,7 @@ #ifndef FUSE_CORE_MESSAGE_BUFFER_IMPL_H #define FUSE_CORE_MESSAGE_BUFFER_IMPL_H +#include #include #include @@ -74,8 +75,9 @@ typename MessageBuffer::message_range MessageBuffer::query( beginning_time_ss << beginning_stamp; std::stringstream ending_time_ss; ending_time_ss << ending_stamp; - throw std::invalid_argument("The beginning_stamp (" + beginning_time_ss.str() + ") must be less than or equal to " - "the ending_stamp (" + ending_time_ss.str() + ")."); + ErrorHandler::getHandler().invalidArgument("The beginning_stamp (" + beginning_time_ss.str() + + ") must be less than or equal to " + "the ending_stamp (" + ending_time_ss.str() + ")."); } // Verify the query is within the bounds of the buffer if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || (ending_stamp > buffer_.back().first)) @@ -91,8 +93,9 @@ typename MessageBuffer::message_range MessageBuffer::query( { available_time_range_ss << "(" << buffer_.front().first << ", " << buffer_.back().first << ")"; } - throw std::out_of_range("The requested time range " + requested_time_range_ss.str() + " is outside the available " - "time range " + available_time_range_ss.str() + "."); + ErrorHandler::getHandler().outOfRangeError("The requested time range " + + requested_time_range_ss.str() + " is outside the available " + "time range " + available_time_range_ss.str() + "."); } // Find the entry that is strictly greater than the requested beginning stamp. If the extended range flag is true, // we will then back up one entry. diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h index dd8d6f9b9..ba627d61b 100644 --- a/fuse_core/include/fuse_core/parameter.h +++ b/fuse_core/include/fuse_core/parameter.h @@ -35,6 +35,7 @@ #define FUSE_CORE_PARAMETER_H #include +#include #include #include @@ -61,7 +62,7 @@ void getParamRequired(const ros::NodeHandle& nh, const std::string& key, T& valu { const std::string error = "Could not find required parameter " + key + " in namespace " + nh.getNamespace(); ROS_FATAL_STREAM(error); - throw std::runtime_error(error); + fuse_core::ErrorHandler::getHandler().runtimeError(error); } } @@ -135,14 +136,15 @@ fuse_core::Matrix getCovarianceDiagonalParam(const ros::Node const auto diagonal_size = diagonal.size(); if (diagonal_size != Size) { - throw std::invalid_argument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + + ErrorHandler::getHandler().invalidArgument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + std::to_string(Size)); } if (std::any_of(diagonal.begin(), diagonal.end(), [](const auto& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) { - throw std::invalid_argument("Invalid negative diagonal values in " + fuse_core::to_string(Vector(diagonal.data()))); + ErrorHandler::getHandler().invalidArgument("Invalid negative diagonal values in " + + fuse_core::to_string(Vector(diagonal.data()))); } return Vector(diagonal.data()).asDiagonal(); diff --git a/fuse_core/include/fuse_core/timestamp_manager.h b/fuse_core/include/fuse_core/timestamp_manager.h index b759e4ece..ee72cd9b0 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.h +++ b/fuse_core/include/fuse_core/timestamp_manager.h @@ -35,6 +35,7 @@ #define FUSE_CORE_TIMESTAMP_MANAGER_H #include +#include #include #include #include diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 99acbcc50..550a5d3f2 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include @@ -195,7 +196,8 @@ void loadSolverOptionsFromROS(const ros::NodeHandle& nh, ceres::Solver::Options& std::string error; if (!solver_options.IsValid(&error)) { - throw std::invalid_argument("Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); + ErrorHandler::getHandler().invalidArgument( + "Invalid solver options in parameter " + nh.getNamespace() + ". Error: " + error); } } diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index d8d00aacf..b92f4750b 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -74,7 +74,8 @@ void TimestampManager::query( && (stamps.front() < motion_model_history_.begin()->first) && (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) { - throw std::invalid_argument("All timestamps must be within the defined buffer length of the motion model"); + ErrorHandler::getHandler().invalidArgument("All timestamps must be within the " \ + "defined buffer length of the motion model"); } // Create a list of all the required timestamps involved in motion model segments that must be created // Add all of the existing timestamps between the first and last input stamp diff --git a/fuse_core/test/test_error_handler.cpp b/fuse_core/test/test_error_handler.cpp new file mode 100644 index 000000000..b45fbe48a --- /dev/null +++ b/fuse_core/test/test_error_handler.cpp @@ -0,0 +1,136 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Clearpath Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include + +#include +#include + + +// Simple toy class for use in testing callback registration +class ErrorTester +{ + public: + ErrorTester() + { + errors_ = 0; + } + + void testFunction(std::string unused) + { + ++errors_; + } + + int errors_; +}; + +TEST(ErrorHandler, DefaultErrorHandler) +{ + // Tests are not guaranteed to be called in order. Ensure cleanup + fuse_core::ErrorHandler::getHandler().reset(); + + // Verify that default behavior for global throws exceptions + // Many other unit tests use expected exceptions for their unit tests. This test is to see if they've been broken + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid"), std::invalid_argument); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range"), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().logicError("Expected logic error"), std::logic_error); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error"), std::runtime_error); + + // Verify that callbacks can be registered as expected + { + ErrorTester test; + + fuse_core::ErrorHandler::getHandler().registerinvalidArgumentErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerRuntimeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerOutOfRangeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + fuse_core::ErrorHandler::getHandler().registerLogicErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1)); + + fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid argument"); + fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range"); + fuse_core::ErrorHandler::getHandler().logicError("Expected logic error"); + fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error"); + + EXPECT_EQ(test.errors_, 4); + } +} + +TEST(ErrorHandler, ContextErrorHandler) +{ + // Tests are not guaranteed to be called in order. Ensure cleanup + fuse_core::ErrorHandler::getHandler().reset(); + + // By default, no context except for the generic one has a defined error callback + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid", + fuse_core::ErrorContext::CORE), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of range", + fuse_core::ErrorContext::CONSTRAINT), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().logicError("Expected logic error", + fuse_core::ErrorContext::GRAPH), std::out_of_range); + EXPECT_THROW(fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error", + fuse_core::ErrorContext::LOSS), std::out_of_range); + + // Verify that callbacks can be registered as expected for different contexts + { + ErrorTester test; + + fuse_core::ErrorHandler::getHandler().registerinvalidArgumentErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::CORE); + fuse_core::ErrorHandler::getHandler().registerRuntimeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::CONSTRAINT); + fuse_core::ErrorHandler::getHandler().registerOutOfRangeErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::GRAPH); + fuse_core::ErrorHandler::getHandler().registerLogicErrorCb( + std::bind(&ErrorTester::testFunction, &test, std::placeholders::_1), fuse_core::ErrorContext::LOSS); + + fuse_core::ErrorHandler::getHandler().invalidArgument("Expected invalid Argument", fuse_core::ErrorContext::CORE); + fuse_core::ErrorHandler::getHandler().outOfRangeError("Expected out of Range", fuse_core::ErrorContext::GRAPH); + fuse_core::ErrorHandler::getHandler().logicError("Expected logic error", fuse_core::ErrorContext::LOSS); + fuse_core::ErrorHandler::getHandler().runtimeError("Expected runtime error", fuse_core::ErrorContext::CONSTRAINT); + + EXPECT_EQ(test.errors_, 4); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index b5d421cbc..e2bbebdd2 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -33,6 +33,7 @@ */ #include +#include #include #include @@ -127,8 +128,10 @@ bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) { if (!variableExists(variable_uuid)) { - throw std::logic_error("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + - ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + ")."); + fuse_core::ErrorHandler::getHandler().logicError("Attempting to add a constraint (" + + fuse_core::uuid::to_string(constraint->uuid()) + + ") that uses an unknown variable (" + + fuse_core::uuid::to_string(variable_uuid) + ")."); } } // Add the constraint to the list of known constraints @@ -165,7 +168,9 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con auto constraints_iter = constraints_.find(constraint_uuid); if (constraints_iter == constraints_.end()) { - throw std::out_of_range("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().outOfRangeError("The constraint UUID " + + fuse_core::uuid::to_string(constraint_uuid) + + " does not exist."); } return *constraints_iter->second; } @@ -207,8 +212,12 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons else { // We only want to throw if the requested variable does not exist. - throw std::logic_error("Attempting to access constraints connected to variable (" + fuse_core::ErrorHandler::getHandler().logicError("Attempting to access constraints connected to variable (" + fuse_core::uuid::to_string(variable_uuid) + "), but that variable does not exist in this graph."); + // Only here to satisfy compiler warnings. Be warned that if your ErrorHandler + // doesn't throw an exception of some kind, + // this may exhibit undesirable behavior. + return fuse_core::Graph::const_constraint_range(); } } @@ -245,7 +254,8 @@ bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end() && !cross_reference_iter->second.empty()) { - throw std::logic_error("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + fuse_core::ErrorHandler::getHandler().logicError("Attempting to remove a variable (" + + fuse_core::uuid::to_string(variable_uuid) + ") that is used by existing constraints (" + fuse_core::uuid::to_string(cross_reference_iter->second.front()) + " plus " + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); } @@ -264,7 +274,9 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl auto variables_iter = variables_.find(variable_uuid); if (variables_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(variable_uuid) + + " does not exist."); } return *variables_iter->second; } @@ -337,14 +349,16 @@ void HashGraph::getCovariance( auto variable1_iter = variables_.find(request.first); if (variable1_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.first) - + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(request.first) + + " does not exist."); } auto variable2_iter = variables_.find(request.second); if (variable2_iter == variables_.end()) { - throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.second) - + " does not exist."); + fuse_core::ErrorHandler::getHandler().runtimeError("The variable UUID " + + fuse_core::uuid::to_string(request.second) + + " does not exist."); } // Both variables exist. Continue processing. // Create the output covariance matrix @@ -374,7 +388,7 @@ void HashGraph::getCovariance( ceres::Covariance covariance(options); if (!covariance.Compute(unique_covariance_blocks, &problem)) { - throw std::runtime_error("Could not compute requested covariance blocks."); + fuse_core::ErrorHandler::getHandler().runtimeError("Could not compute requested covariance blocks."); } // Populate the computed covariance blocks into the output variable. if (use_tangent_space) @@ -388,7 +402,7 @@ void HashGraph::getCovariance( output_matrix.data())) { const auto& request = covariance_requests.at(i); - throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::ErrorHandler::getHandler().runtimeError("Could not get covariance block for variable UUIDs " + fuse_core::uuid::to_string(request.first) + " and " + fuse_core::uuid::to_string(request.second) + "."); } @@ -405,7 +419,7 @@ void HashGraph::getCovariance( output_matrix.data())) { const auto& request = covariance_requests.at(i); - throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::ErrorHandler::getHandler().runtimeError("Could not get covariance block for variable UUIDs " + fuse_core::uuid::to_string(request.first) + " and " + fuse_core::uuid::to_string(request.second) + "."); } diff --git a/fuse_models/include/fuse_models/common/sensor_config.h b/fuse_models/include/fuse_models/common/sensor_config.h index ad6968c73..96ffa68f0 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.h +++ b/fuse_models/include/fuse_models/common/sensor_config.h @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -66,7 +67,7 @@ inline void throwDimensionError(const std::string& dimension) { std::string error = "Dimension " + dimension + " is not valid for this type."; ROS_ERROR_STREAM(error); - throw std::runtime_error(error); + fuse_core::ErrorHandler::getHandler().runtimeError(error); } /** diff --git a/fuse_models/include/fuse_models/common/sensor_proc.h b/fuse_models/include/fuse_models/common/sensor_proc.h index 8629b4b9a..ce20c098b 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.h +++ b/fuse_models/include/fuse_models/common/sensor_proc.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -196,18 +197,18 @@ inline void validatePartialMeasurement( { if (!mean_partial.allFinite()) { - throw std::runtime_error("Invalid partial mean " + fuse_core::to_string(mean_partial)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid partial mean " + fuse_core::to_string(mean_partial)); } if (!fuse_core::isSymmetric(covariance_partial, precision)) { - throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-symmetric partial covariance matrix\n" + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } if (!fuse_core::isPositiveDefinite(covariance_partial)) { - throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-positive-definite partial covariance matrix\n" + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } } diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h index 08ea2a71a..327965631 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -77,8 +78,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (sigma_vector.size() != 8) { - throw std::invalid_argument("The supplied initial_sigma parameter must be length 8, but is actually length " + - std::to_string(sigma_vector.size())); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must " + "be length 8, but is actually length " + + std::to_string(sigma_vector.size())); } auto is_sigma_valid = [](const double sigma) { @@ -86,8 +88,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_sigma parameter must " + "contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } @@ -97,8 +100,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase { if (state_vector.size() != 8) { - throw std::invalid_argument("The supplied initial_state parameter must be length 8, but is actually length " + - std::to_string(state_vector.size())); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter " + "must be length 8, but is actually length " + + std::to_string(state_vector.size())); } auto is_state_valid = [](const double state) { @@ -106,8 +110,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase }; if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + fuse_core::ErrorHandler::getHandler().invalidArgument("The supplied initial_state parameter must " + "contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 80d5b56f1..b0d949a63 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -33,6 +33,7 @@ */ #include +#include #include @@ -113,7 +114,7 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) // Verify we are in the correct state to process set graph requests if (!started_) { - throw std::runtime_error("Attempting to set the graph while the sensor is stopped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set the graph while the sensor is stopped."); } // Deserialize the graph message @@ -122,12 +123,12 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) // Validate the requested graph before we do anything if (boost::empty(graph->getConstraints())) { - throw std::runtime_error("Attempting to set a graph with no constraints."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set a graph with no constraints."); } if (boost::empty(graph->getVariables())) { - throw std::runtime_error("Attempting to set a graph with no variables."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set a graph with no variables."); } // Tell the optimizer to reset before providing the initial state @@ -143,7 +144,8 @@ void GraphIgnition::process(const fuse_msgs::SerializedGraph& msg) if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + + reset_client_.getService() + "' service."); } } diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 1da323519..537c4a610 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -99,13 +100,13 @@ inline void validateCovariance(const Eigen::DenseBase& covariance, { if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-symmetric partial covariance matrix\n" + fuse_core::to_string(covariance, Eigen::FullPrecision)); } if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::ErrorHandler::getHandler().runtimeError("Non-positive-definite partial covariance matrix\n" + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -150,22 +151,23 @@ void Unicycle2D::StateHistoryElement::validate() const { if (!std::isfinite(pose)) { - throw std::runtime_error("Invalid pose " + std::to_string(pose)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid pose " + std::to_string(pose)); } if (!std::isfinite(velocity_linear)) { - throw std::runtime_error("Invalid linear velocity " + std::to_string(velocity_linear)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear velocity " + std::to_string(velocity_linear)); } if (!std::isfinite(velocity_yaw)) { - throw std::runtime_error("Invalid yaw velocity " + std::to_string(velocity_yaw)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid yaw velocity " + std::to_string(velocity_yaw)); } if (!std::isfinite(acceleration_linear)) { - throw std::runtime_error("Invalid linear acceleration " + std::to_string(acceleration_linear)); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid linear acceleration " + + std::to_string(acceleration_linear)); } } @@ -198,7 +200,7 @@ void Unicycle2D::onInit() if (process_noise_diagonal.size() != 8) { - throw std::runtime_error("Process noise diagonal must be of length 8!"); + fuse_core::ErrorHandler::getHandler().runtimeError("Process noise diagonal must be of length 8!"); } process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); @@ -213,7 +215,8 @@ void Unicycle2D::onInit() if (buffer_length < 0.0) { - throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid negative buffer length of " + + std::to_string(buffer_length) + " specified."); } buffer_length_ = (buffer_length == 0.0) ? ros::DURATION_MAX : ros::Duration(buffer_length); @@ -488,7 +491,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid state #1: " + std::string(ex.what())); } try @@ -497,7 +500,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid state #2: " + std::string(ex.what())); } try @@ -506,7 +509,7 @@ void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const St } catch (const std::runtime_error& ex) { - throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); + fuse_core::ErrorHandler::getHandler().runtimeError("Invalid process noise covariance: " + std::string(ex.what())); } } diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 5f162ab40..79c2646a1 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -181,12 +182,12 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& // Verify we are in the correct state to process set pose requests if (!started_) { - throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { - throw std::invalid_argument("Attempting to set the pose to an invalid position (" + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + std::to_string(pose.pose.pose.position.y) + ")."); } @@ -196,7 +197,7 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); if (std::abs(orientation_norm - 1.0) > 1.0e-3) { - throw std::invalid_argument("Attempting to set the pose to an invalid orientation (" + + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + @@ -207,20 +208,25 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose.pose.covariance[6], pose.pose.covariance[7]; if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matri\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-symmetric position covariance matri\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + + "."); } if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + + "."); } auto orientation_cov = fuse_core::Matrix1d(); orientation_cov << pose.pose.covariance[35]; if (orientation_cov(0) <= 0.0) { - throw std::invalid_argument("Attempting to set the pose with a non-positive-definite orientation covariance " - "matrix " + fuse_core::to_string(orientation_cov) + "."); + fuse_core::ErrorHandler::getHandler().invalidArgument("Attempting to set the pose with a " + "non-positive-definite orientation covariance " + "matrix " + fuse_core::to_string(orientation_cov) + "."); } // Tell the optimizer to reset before providing the initial state @@ -236,7 +242,9 @@ void Unicycle2DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& if (!reset_client_.call(srv)) { // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to call the '" + + reset_client_.getService() + + "' service."); } } diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 0fa6928a2..22e8ce7f0 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -113,6 +113,7 @@ class Optimizer */ virtual ~Optimizer(); + protected: // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type rather annoying // as it is not equivalent to Class::UniquePtr @@ -176,6 +177,13 @@ class Optimizer const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; + // /** + // * @brief Method exposed for resetting the optimizer in the event of an error. Replaces + // * the need to throw exceptions when things go wrong. + // * @param[in] err_msg A specific error message that gives a reason for the reset + // */ + // virtual void resetOptimizer(const std::string& err_msg) = 0; + /** * @brief Configure the motion model plugins specified on the parameter server * diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 203ee1769..814bb0079 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -142,8 +143,9 @@ void Optimizer::loadMotionModels() || (!motion_model.hasMember("name")) || (!motion_model.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " - "-{name: string, type: string}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be " + "a list of the form: " + "-{name: string, type: string}"); } motion_model_configs.emplace_back(static_cast(motion_model["name"]), @@ -159,8 +161,9 @@ void Optimizer::loadMotionModels() if ( (motion_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!motion_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'motion_models' parameter should be a struct of the form: " - "{string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be" + " a struct of the form: " + "{string: {type: string}}"); } motion_model_configs.emplace_back(static_cast(motion_model.first), @@ -169,7 +172,7 @@ void Optimizer::loadMotionModels() } else { - throw std::invalid_argument("The 'motion_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'motion_models' parameter should be a list of the form: " "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); } @@ -208,8 +211,10 @@ void Optimizer::loadSensorModels() || (!sensor_model.hasMember("name")) || (!sensor_model.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " - "-{name: string, type: string, motion_models: [name1, name2, ...]}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be " + " a list of the form: " + "-{name: string, type: string, " + "motion_models: [name1, name2, ...]}"); } sensor_model_configs.emplace_back(static_cast(sensor_model["name"]), @@ -226,8 +231,10 @@ void Optimizer::loadSensorModels() if ( (sensor_model_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!sensor_model_config.hasMember("type"))) { - throw std::invalid_argument("The 'sensor_models' parameter should be a struct of the form: " - "{string: {type: string, motion_models: [name1, name2, ...]}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be " + "a struct of the form: " + "{string: {type: string, " + "motion_models: [name1, name2, ...]}}"); } sensor_model_configs.emplace_back(static_cast(sensor_model.first), @@ -236,7 +243,7 @@ void Optimizer::loadSensorModels() } else { - throw std::invalid_argument("The 'sensor_models' parameter should be a list of the form: " + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'sensor_models' parameter should be a list of the form: " "-{name: string, type: string, motion_models: [name1, name2, ...]} " "or a struct of the form: " "{string: {type: string, motion_models: [name1, name2, ...]}}"); @@ -301,8 +308,9 @@ void Optimizer::loadPublishers() || (!publisher.hasMember("name")) || (!publisher.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a list of the form: " + "-{name: string, type: string}"); } publisher_configs.emplace_back(static_cast(publisher["name"]), @@ -319,8 +327,9 @@ void Optimizer::loadPublishers() if ( (publisher_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) || (!publisher_config.hasMember("type"))) { - throw std::invalid_argument("The 'publishers' parameter should be a struct of the form: " - "{string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a struct of the form: " + "{string: {type: string}}"); } publisher_configs.emplace_back(static_cast(publisher.first), @@ -329,8 +338,11 @@ void Optimizer::loadPublishers() } else { - throw std::invalid_argument("The 'publishers' parameter should be a list of the form: " - "-{name: string, type: string} or a struct of the form: {string: {type: string}}"); + fuse_core::ErrorHandler::getHandler().invalidArgument("The 'publishers' parameter should be " + "a list of the form: " + "-{name: string, type: string} or " + "a struct of the form: " + "{string: {type: string}}"); } for (const auto& config : publisher_configs) diff --git a/fuse_viz/include/fuse_viz/conversions.h b/fuse_viz/include/fuse_viz/conversions.h index 8f8d16b41..4d7dc86de 100644 --- a/fuse_viz/include/fuse_viz/conversions.h +++ b/fuse_viz/include/fuse_viz/conversions.h @@ -35,6 +35,7 @@ #ifndef FUSE_VIZ_CONVERSIONS_H #define FUSE_VIZ_CONVERSIONS_H +#include #include #include #include @@ -137,16 +138,18 @@ inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UU const auto position = dynamic_cast(&graph.getVariable(position_uuid)); if (!position) { - throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + - " from graph as fuse_variables::Position2DStamped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + + fuse_core::uuid::to_string(position_uuid) + + " from graph as fuse_variables::Position2DStamped."); } const auto orientation = dynamic_cast(&graph.getVariable(orientation_uuid)); if (!orientation) { - throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + - " from graph as fuse_variables::Orientation2DStamped."); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to get variable " + + fuse_core::uuid::to_string(orientation_uuid) + + " from graph as fuse_variables::Orientation2DStamped."); } return getPose(*position, *orientation); diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index 90679f5e2..2d548c288 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -51,6 +51,7 @@ #include #include +#include #include #include #include @@ -249,7 +250,7 @@ void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::Co { delete constraint_source_property; - throw std::runtime_error("Failed to insert " + description); + fuse_core::ErrorHandler::getHandler().runtimeError("Failed to insert " + description); } show_constraints_property_->addChild(constraint_source_property,