From 044d5dead3fa149f10cf4c2ec4030f57ae5778f9 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 12 Aug 2024 13:08:30 -0400 Subject: [PATCH] do not run calibration if no feature finders (#167) due to misconfiguration (for instance, camera_info topic is wrong) the finders may not initialize but the robot will move through all the poses, say it captured all of them, and then have no observations in the output bagfile --- .../include/robot_calibration/finders/loader.hpp | 11 +++++++++++ robot_calibration/src/nodes/calibrate.cpp | 6 +++++- 2 files changed, 16 insertions(+), 1 deletion(-) 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();