From 0cced4a6c3bdc7844068b970a577521828efff46 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Tue, 28 May 2019 14:18:10 +0200
Subject: [PATCH] Define where the output file should go by a parameter

---
 ur_calibration/src/calibration_correction.cpp | 74 ++++++++++++++++++-
 1 file changed, 71 insertions(+), 3 deletions(-)

diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp
index 7f77b2a..33670db 100644
--- a/ur_calibration/src/calibration_correction.cpp
+++ b/ur_calibration/src/calibration_correction.cpp
@@ -23,6 +23,11 @@
 
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_listener.h>
+#include <ros/package.h>
+
+#include <boost/filesystem.hpp>
+
+namespace fs = boost::filesystem;
 
 using namespace ur_driver;
 using namespace primary_interface;
@@ -30,12 +35,65 @@ using namespace ur_calibration;
 
 static const int UR_PRIMARY_PORT = 30001;
 
+class ParamaterMissingException : public ros::Exception
+{
+public:
+  ParamaterMissingException(const std::string& name)
+    : Exception("Cannot find required parameter " + name + " on the parameter server.")
+  {
+  }
+};
+
+template <typename T>
+T getRequiredParameter(const ros::NodeHandle& nh, const std::string& param_name)
+{
+  T ret_val;
+  if (nh.hasParam(param_name))
+  {
+    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;
+  ros::NodeHandle nh("~");
+
+  std::string subfolder = nh.param<std::string>("subfolder_name", "etc");
+  std::string robot_ip, robot_name, output_package_name;
+  try
+  {
+    // 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());
+    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;
+  }
 
-  std::string robot_ip = "192.168.56.101";
   comm::URStream<PackageHeader> stream(robot_ip, UR_PRIMARY_PORT);
   primary_interface::PrimaryParser parser;
   comm::URProducer<PackageHeader> prod(stream, parser);
@@ -52,7 +110,17 @@ int main(int argc, char* argv[])
   pipeline.stop();
 
   std::ofstream file;
-  file.open("test.yaml");
+  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();