auto calibration node and service

CMakeLists.txt

@@ -20,12 +20,22 @@ endif()
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  message_generation
+  std_msgs
   tf_conversions
   roscpp
   tf
   trajectory_msgs
   #vrep_common
 )
+
+add_service_files(FILES
+  HandEyeCalibCommand.srv)
+
+generate_messages(
+  DEPENDENCIES std_msgs)
+
 find_package(Gflags REQUIRED)
 find_package(Glog REQUIRED)
 
@@ -208,6 +218,10 @@ add_executable(handeye_calib_camodocal
   src/handeye_calibration.cpp
   src/camodocal/calib/HandEyeCalibration.cc)
 
+add_executable(handeye_calib_camodocal_auto
+  src/handeye_calibration_auto.cpp
+  src/camodocal/calib/HandEyeCalibration.cc)
+
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 # add_dependencies(handeye_calib_camodocal_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -217,6 +231,9 @@ message("GLOG = ${GLOG_LIBRARIES}  ${GLOG_LIB}  ${GLOG_LIBS}")
 target_link_libraries(handeye_calib_camodocal
   ${catkin_LIBRARIES} ${GLOG_LIBRARIES}  ${GLOG_LIB}  ${GLOG_LIBS} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES}
 )
+target_link_libraries(handeye_calib_camodocal_auto
+  ${catkin_LIBRARIES} ${GLOG_LIBRARIES}  ${GLOG_LIB}  ${GLOG_LIBS} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES}
+)
 #############
 ## Install ##
 #############

package.xml

@@ -40,15 +40,20 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>std_msgs</build_depend>
   <build_depend>eigen</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>trajectory_msgs</build_depend>
+  <build_depend>libceres-dev</build_depend>
+  <build_depend>message_generation</build_depend>
   <!--build_depend>vrep_common</build_depend -->
+  <run_depend>std_msgs</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>trajectory_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
   <!--run_depend>vrep_common</run_depend -->
 
 

src/handeye_calibration.cpp

@@ -2,6 +2,7 @@
 #include "ceres/types.h"
 #include <camodocal/calib/HandEyeCalibration.h>
 #include <eigen3/Eigen/Geometry>
+#include <fstream>
 #include <opencv2/core/eigen.hpp>
 #include <ros/ros.h>
 #include <termios.h>
@@ -104,7 +105,7 @@ int readTransformPairsFromFile(std::string filename, EigenAffineVector& t1,
         }
         fs.release();
     } else {
-        std::cerr << "failed to open input file " << filename << "\n";
+        ROS_ERROR("failed to open input file %s", filename.c_str());
         return 1;
     }
     return 0;
@@ -429,6 +430,7 @@ int main(int argc, char** argv) {
     std::string calibratedTransformFile;
     bool loadTransformsFromFile = false;
     bool addSolverSummary = false;
+    std::string output_launch_filename;
 
     // getting TF names
     nh.param("ARTagTF", ARTagTFname, std::string("/camera_2/ar_marker_0"));
@@ -437,6 +439,8 @@ int main(int argc, char** argv) {
     nh.param("baseTF", baseTFname, std::string("/base_link"));
     nh.param("add_solver_summary", addSolverSummary, false);
     nh.param("load_transforms_from_file", loadTransformsFromFile, false);
+    nh.param("output_launch_filename", output_launch_filename,
+             std::string("/tmp/maker_to_ee_static_transform_publisher.launch"));
     nh.param("transform_pairs_record_filename", transformPairsRecordFile,
              std::string("TransformPairsInput.yml"));
     nh.param("transform_pairs_load_filename", transformPairsLoadFile,
@@ -450,10 +454,38 @@ int main(int argc, char** argv) {
         std::cerr << "Transform pairs loading file: " << transformPairsLoadFile
                   << "\n";
         EigenAffineVector t1, t2;
-        readTransformPairsFromFile(transformPairsLoadFile, t1, t2);
-        auto result =
-            estimateHandEye(t1, t2, calibratedTransformFile, addSolverSummary);
+        int ret = readTransformPairsFromFile(transformPairsLoadFile, t1, t2);
+        if (ret == 0) {
+            auto result = estimateHandEye(t1, t2, calibratedTransformFile,
+                                          addSolverSummary);
 
+            Eigen::Transform<double, 3, Eigen::Affine> resultAffine(result);
+            Eigen::Quaternion<double> quaternionResult(resultAffine.rotation());
+            double x = resultAffine.translation().transpose()[0];
+            double y = resultAffine.translation().transpose()[1];
+            double z = resultAffine.translation().transpose()[2];
+
+            std::ofstream outputfile(output_launch_filename);
+            outputfile << "<launch>" << std::endl;
+            outputfile << "  <arg name=\"ee_frame\" default=\"" << EETFname
+                       << "\" />" << std::endl;
+            outputfile << "  <arg name=\"marker_frame\" default=\""
+                       << ARTagTFname << "\" />" << std::endl;
+            outputfile << "  <node name=\"endpoint_to_marker\"" << std::endl;
+            outputfile
+                << "        pkg=\"tf\" type=\"static_transform_publisher\""
+                << std::endl;
+            outputfile << "        args=\"";
+            outputfile << x << " " << y << " " << z << std::endl;
+            outputfile << "              " << quaternionResult.x() << " "
+                       << quaternionResult.y() << " " << quaternionResult.z()
+                       << " " << quaternionResult.w() << " " << std::endl;
+            outputfile
+                << "              $(arg ee_frame) $(arg marker_frame) 1000\" />"
+                << std::endl;
+            outputfile << "</launch>" << std::endl;
+            outputfile.close();
+        }
         return 0;
     }