WIP: Added calibration_from_file executable

ur_calibration/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED)
 
 
 ## Uncomment this if the package has a setup.py. This macro ensures
@@ -110,6 +110,7 @@ catkin_package(
     roscpp
     ur_rtde_driver
   DEPENDS
+    boost
     yaml-cpp
 )
 
@@ -123,6 +124,7 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
 )
 
 add_executable(calibration_correction
@@ -139,6 +141,19 @@ target_link_libraries(calibration_correction
   yaml-cpp
 )
 
+add_executable(calibration_from_file
+  src/calibration.cpp
+  src/calibration_from_file.cpp
+)
+
+add_dependencies(calibration_from_file ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(calibration_from_file
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
+
 #############
 ## Install ##
 #############

ur_calibration/src/calibration_from_file.cpp

@@ -0,0 +1,123 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-09-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#include <ur_calibration/calibration.h>
+
+#include <boost/filesystem.hpp>
+#include <boost/property_tree/ini_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/spirit/include/qi.hpp>
+
+using namespace ur_calibration;
+namespace fs = boost::filesystem;
+
+template <typename Iterator>
+bool parse_numbers_into_vector(Iterator first, Iterator last, std::vector<double>& v)
+{
+  using boost::spirit::ascii::space;
+  using boost::spirit::qi::_1;
+  using boost::spirit::qi::double_;
+  using boost::spirit::qi::phrase_parse;
+
+  bool r = phrase_parse(first, last, ('[' >> double_ % ',' >> ']'), space, v);
+
+  if (first != last)  // fail if we did not get a full match
+    return false;
+  return r;
+}
+
+std::vector<double> parse_tree_entry(const boost::property_tree::ptree& p)
+{
+  const std::string& numbers = p.get_value<std::string>();
+
+  std::vector<double> result;
+  if (!parse_numbers_into_vector(numbers.begin(), numbers.end(), result))
+  {
+    throw std::runtime_error("Value could not be parsed into vector of doubles.");
+  }
+  return result;
+}
+
+DHRobot robotFromTree(const boost::property_tree::ptree& p)
+{
+  const boost::property_tree::ptree& mounting = p.get_child("mounting");
+  const boost::property_tree::ptree& dh = p.get_child("DH");
+
+  DHRobot my_robot;
+
+  std::vector<double> a = parse_tree_entry(dh.get_child("a"));
+  std::vector<double> d = parse_tree_entry(dh.get_child("d"));
+  std::vector<double> theta = { 0, 0, 0, 0, 0, 0 };
+  std::vector<double> alpha = parse_tree_entry(dh.get_child("alpha"));
+  std::vector<double> delta_a = parse_tree_entry(mounting.get_child("delta_a"));
+  std::vector<double> delta_d = parse_tree_entry(mounting.get_child("delta_d"));
+  std::vector<double> delta_theta = parse_tree_entry(mounting.get_child("delta_theta"));
+  std::vector<double> delta_alpha = parse_tree_entry(mounting.get_child("delta_alpha"));
+  for (size_t i = 0; i < a.size(); ++i)
+  {
+    my_robot.segments_.push_back(
+        DHSegment(d[i] + delta_d[i], a[i] + delta_a[i], theta[i] + delta_theta[i], alpha[i] + delta_alpha[i]));
+  }
+
+  return my_robot;
+}
+
+int main(int argc, char* argv[])
+{
+  ros::init(argc, argv, "ur_calibration");
+  ros::NodeHandle nh("~");
+
+  std::string config_filename;
+  nh.getParam("config_filename", config_filename);
+
+  ROS_INFO_STREAM("Config filename: " << config_filename);
+
+  DHRobot my_robot;
+  boost::property_tree::ptree config_tree;
+  try
+  {
+    boost::property_tree::ini_parser::read_ini(config_filename, config_tree);
+
+    my_robot = robotFromTree(config_tree);
+  }
+  catch (const boost::property_tree::ptree_error& ex)
+  {
+    ROS_ERROR_STREAM("An error occurred while reading the config data: " << ex.what());
+  }
+  Calibration calibration(my_robot);
+  calibration.correctChain();
+
+  YAML::Node yaml_node = calibration.toYaml();
+
+  fs::path output_filename = fs::path("/tmp/robot_calibration.yaml");
+  std::ofstream file(output_filename.string());
+  if (file.is_open())
+  {
+    file << yaml_node;
+  }
+  return 0;
+}