Improved handling of invalid save paths

ur_calibration/src/calibration_correction.cpp

@@ -44,57 +44,45 @@ public:
   }
 };
 
-template <typename T>
-T getRequiredParameter(const ros::NodeHandle& nh, const std::string& param_name)
+class CalibrationCorrection
 {
-  T ret_val;
-  if (nh.hasParam(param_name))
+public:
+  CalibrationCorrection(const ros::NodeHandle& nh) : nh_(nh)
   {
-    nh.getParam(param_name, ret_val);
-  }
-  else
-  {
-    throw ParamaterMissingException(nh.resolveName(param_name));
-  }
-
-  return ret_val;
-}
-
-int main(int argc, char* argv[])
-{
-  ros::init(argc, argv, "ur_calibration");
-  ros::NodeHandle nh("~");
-
-  std::string subfolder = nh.param<std::string>("subfolder_name", "etc");
-  std::string robot_ip, robot_name, output_package_name;
+    subfolder_ = nh_.param<std::string>("subfolder_name", "etc");
+    std::string output_package_name;
     try
     {
       // The robot's IP address
-    robot_ip = getRequiredParameter<std::string>(nh, "robot_ip");
+      robot_ip_ = getRequiredParameter<std::string>("robot_ip");
 
       // Name with which the robot will be referenced. Will be used for the filename the calibration
       // data is stored in. This can be any arbitrary name. If left empty, the robot's serial number
       // will be used.
-    robot_name = getRequiredParameter<std::string>(nh, "robot_name");
+      robot_name_ = getRequiredParameter<std::string>("robot_name");
 
       // The resulting parameter file will be stored inside
       // <output_package_name>/subfolder/<robot_name>_calibration.yaml
-    output_package_name = getRequiredParameter<std::string>(nh, "output_package_name");
+      output_package_name = getRequiredParameter<std::string>("output_package_name");
     }
     catch (const ParamaterMissingException& e)
     {
       ROS_FATAL_STREAM(e.what());
-    return -1;
+      exit(1);
     }
-
-  std::string package_path = ros::package::getPath(output_package_name);
-  if (package_path == "")
+    package_path_ = ros::package::getPath(output_package_name);
+    if (package_path_ == "")
     {
       ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there.");
-    return -1;
+      exit(1);
+    }
   }
 
-  comm::URStream<PackageHeader> stream(robot_ip, UR_PRIMARY_PORT);
+  virtual ~CalibrationCorrection() = default;
+
+  void run()
+  {
+    comm::URStream<PackageHeader> stream(robot_ip_, UR_PRIMARY_PORT);
     primary_interface::PrimaryParser parser;
     comm::URProducer<PackageHeader> prod(stream, parser);
     CalibrationConsumer consumer;
@@ -107,25 +95,90 @@ int main(int argc, char* argv[])
     {
       ros::Duration(0.1).sleep();
     }
-  pipeline.stop();
+    calibration_data_.reset(new YAML::Node);
+    *calibration_data_ = consumer.getCalibrationParameters();
+  }
 
-  std::ofstream file;
-  fs::path dst_path = fs::path(package_path) / fs::path(subfolder);
+  bool writeCalibrationData()
+  {
+    if (calibration_data_ == nullptr)
+    {
+      ROS_ERROR_STREAM("Calibration data not yet set.");
+      return false;
+    }
+
+    ROS_INFO_STREAM("Writing calibration data");
+    fs::path dst_path = fs::path(package_path_) / fs::path(subfolder_);
     if (!fs::exists(dst_path))
+    {
+      try
       {
         fs::create_directory(dst_path);
       }
-  fs::path output_filename = dst_path / fs::path(robot_name + "_calibration.yaml");
+      catch (const boost::filesystem::filesystem_error& e)
+      {
+        ROS_ERROR_STREAM("Could not create " << dst_path << ". Reason: " << e.what());
+        return false;
+      }
+    }
+    fs::path output_filename = dst_path / fs::path(robot_name_ + "_calibration.yaml");
     if (fs::exists(output_filename))
     {
       ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting.");
     }
-  file.open(output_filename.string());
-  YAML::Node calibration_parameters = consumer.getCalibrationParameters();
-  file << calibration_parameters;
-  file.close();
+    std::ofstream file(output_filename.string());
+    if (file.is_open())
+    {
+      file << *calibration_data_;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("Failed writing the file. Do you have the correct rights?");
+      return false;
+    }
+    ROS_INFO_STREAM("Wrote output to " << output_filename);
 
-  ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters);
+    return true;
+  }
 
+private:
+  template <typename T>
+  T getRequiredParameter(const std::string& param_name) const
+  {
+    T ret_val;
+    if (nh_.hasParam(param_name))
+    {
+      nh_.getParam(param_name, ret_val);
+    }
+    else
+    {
+      throw ParamaterMissingException(nh_.resolveName(param_name));
+    }
+
+    return ret_val;
+  }
+
+  ros::NodeHandle nh_;
+  std::string robot_ip_;
+  std::string robot_name_;
+  std::string package_path_;
+  std::string subfolder_;
+
+  std::unique_ptr<YAML::Node> calibration_data_;
+};
+
+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())
+  {
+    ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details.");
+    return -1;
+  }
+  ROS_INFO("Calibration correction done");
   return 0;
 }