diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp
index 33670db..eee1eb5 100644
--- a/ur_calibration/src/calibration_correction.cpp
+++ b/ur_calibration/src/calibration_correction.cpp
@@ -44,88 +44,141 @@ 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));
+    subfolder_ = nh_.param<std::string>("subfolder_name", "etc");
+    std::string output_package_name;
+    try
+    {
+      // The robot's IP address
+      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>("robot_name");
+
+      // The resulting parameter file will be stored inside
+      // <output_package_name>/subfolder/<robot_name>_calibration.yaml
+      output_package_name = getRequiredParameter<std::string>("output_package_name");
+    }
+    catch (const ParamaterMissingException& e)
+    {
+      ROS_FATAL_STREAM(e.what());
+      exit(1);
+    }
+    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.");
+      exit(1);
+    }
   }
 
-  return ret_val;
-}
+  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;
+
+    comm::INotifier notifier;
+
+    comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
+    pipeline.run();
+    while (!consumer.isCalibrated())
+    {
+      ros::Duration(0.1).sleep();
+    }
+    calibration_data_.reset(new YAML::Node);
+    *calibration_data_ = consumer.getCalibrationParameters();
+  }
+
+  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);
+      }
+      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.");
+    }
+    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);
+
+    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("~");
 
-  std::string subfolder = nh.param<std::string>("subfolder_name", "etc");
-  std::string robot_ip, robot_name, output_package_name;
-  try
+  CalibrationCorrection my_calibration_correction(nh);
+  my_calibration_correction.run();
+  if (!my_calibration_correction.writeCalibrationData())
   {
-    // The robot's IP address
-    robot_ip = getRequiredParameter<std::string>(nh, "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");
-
-    // 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");
-  }
-  catch (const ParamaterMissingException& e)
-  {
-    ROS_FATAL_STREAM(e.what());
+    ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details.");
     return -1;
   }
-
-  std::string 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;
-  }
-
-  comm::URStream<PackageHeader> stream(robot_ip, UR_PRIMARY_PORT);
-  primary_interface::PrimaryParser parser;
-  comm::URProducer<PackageHeader> prod(stream, parser);
-  CalibrationConsumer consumer;
-
-  comm::INotifier notifier;
-
-  comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
-  pipeline.run();
-  while (!consumer.isCalibrated())
-  {
-    ros::Duration(0.1).sleep();
-  }
-  pipeline.stop();
-
-  std::ofstream file;
-  fs::path dst_path = fs::path(package_path) / fs::path(subfolder);
-  if (!fs::exists(dst_path))
-  {
-    fs::create_directory(dst_path);
-  }
-  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();
-
-  ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters);
-
+  ROS_INFO("Calibration correction done");
   return 0;
 }