diff --git a/robot_calibration/include/robot_calibration/finders/loader.hpp b/robot_calibration/include/robot_calibration/finders/loader.hpp index 8e78f0c2..5771d728 100644 --- a/robot_calibration/include/robot_calibration/finders/loader.hpp +++ b/robot_calibration/include/robot_calibration/finders/loader.hpp @@ -90,6 +90,17 @@ class FeatureFinderLoader { features[name] = finder; } + else + { + RCLCPP_ERROR(logger, "Feature finder %s failed to initialize", name.c_str()); + } + } + + // Make sure at least one finder loaded correctly + if (features.empty()) + { + RCLCPP_FATAL(logger, "No feature finders loaded"); + return false; } return true; diff --git a/robot_calibration/src/nodes/calibrate.cpp b/robot_calibration/src/nodes/calibrate.cpp index f98e590e..e9c7892f 100644 --- a/robot_calibration/src/nodes/calibrate.cpp +++ b/robot_calibration/src/nodes/calibrate.cpp @@ -87,7 +87,11 @@ int main(int argc, char** argv) { // No name provided for a calibration bag file, must do capture robot_calibration::CaptureManager capture_manager; - capture_manager.init(node); + if (!capture_manager.init(node)) + { + // Error will be printed in function + return -1; + } // Save URDF for calibration/export step description_msg.data = capture_manager.getUrdf();