From 6aa2670eba4a9d7b9f218ab906de391ec29f81a7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 3 Jul 2019 10:10:17 +0200 Subject: [PATCH] print an error message in calibration and exit when connection cannot be established --- ur_calibration/src/calibration_correction.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index f460b48..a804e53 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -183,13 +183,20 @@ int main(int argc, char* argv[]) ros::init(argc, argv, "ur_calibration"); ros::NodeHandle nh("~"); - CalibrationCorrection my_calibration_correction(nh); - my_calibration_correction.run(); - if (!my_calibration_correction.writeCalibrationData()) + try { - ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details."); - return -1; + CalibrationCorrection my_calibration_correction(nh); + my_calibration_correction.run(); + if (!my_calibration_correction.writeCalibrationData()) + { + ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details."); + return -1; + } + ROS_INFO("Calibration correction done"); + } + catch (const UrException& e) + { + ROS_ERROR_STREAM(e.what()); } - ROS_INFO("Calibration correction done"); return 0; }