From 65818bb263c26bd17280a8d1dffe958db222a58b Mon Sep 17 00:00:00 2001
From: Anton Tetov <anton@tetov.se>
Date: Wed, 4 Sep 2024 15:00:29 +0200
Subject: [PATCH] eye in hand start

---
 dependencies.repos        |  8 +++++
 launch/eye_in_hand.launch | 67 +++++++++++++++++++++++++++++++++++++++
 package.xml               |  2 ++
 3 files changed, 77 insertions(+)
 create mode 100644 launch/eye_in_hand.launch

diff --git a/dependencies.repos b/dependencies.repos
index 3ba8260..e5c1a54 100644
--- a/dependencies.repos
+++ b/dependencies.repos
@@ -33,3 +33,11 @@ repositories:
     type: git
     url: https://github.com/lucasw/camera_info_manager_py.git
     version: noetic-devel
+  easy_handeye:
+    type: git
+    url: https://github.com/IFL-CAMP/easy_handeye
+    version: ffcc43d1e63cd16b3052f499c681cd8fbc7b71f5
+  charuco_detector:
+    type: git
+    url: https://github.com/carlosmccosta/charuco_detector
+    version: 5941c153dcdff986effb1512ffbb03983dc0748d
diff --git a/launch/eye_in_hand.launch b/launch/eye_in_hand.launch
new file mode 100644
index 0000000..617a133
--- /dev/null
+++ b/launch/eye_in_hand.launch
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<launch xmlns:xs="http://www.w3.org/2001/XMLSchema"
+  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+  xsi:noNamespaceSchemaLocation="https://gist.githubusercontent.com/nalt/dfa2abc9d2e3ae4feb82ca5608090387/raw/roslaunch.xsd">
+
+  <!-- enum  	cv::aruco::PredefinedDictionaryType {
+  cv::aruco::DICT_4X4_50 = 0 ,
+  cv::aruco::DICT_4X4_100 ,
+  cv::aruco::DICT_4X4_250 ,
+  cv::aruco::DICT_4X4_1000 ,
+  cv::aruco::DICT_5X5_50 ,
+  cv::aruco::DICT_5X5_100 ,
+  cv::aruco::DICT_5X5_250 ,
+  cv::aruco::DICT_5X5_1000 ,
+  cv::aruco::DICT_6X6_50 ,
+  cv::aruco::DICT_6X6_100 ,
+  cv::aruco::DICT_6X6_250 ,
+  cv::aruco::DICT_6X6_1000 ,
+  cv::aruco::DICT_7X7_50 ,
+  cv::aruco::DICT_7X7_100 ,
+  cv::aruco::DICT_7X7_250 ,
+  cv::aruco::DICT_7X7_1000 ,
+  cv::aruco::DICT_ARUCO_ORIGINAL ,
+  cv::aruco::DICT_APRILTAG_16h5 ,
+  cv::aruco::DICT_APRILTAG_25h9 ,
+  cv::aruco::DICT_APRILTAG_36h10 ,
+  cv::aruco::DICT_APRILTAG_36h11 ,
+  cv::aruco::DICT_ARUCO_MIP_36h12
+} -->
+
+  <arg name="board" default="large" />
+
+  <include file="$(dirname)/rgbd.launch" />
+  <include file="$(dirname)/robots.launch">
+    <arg name="no_rws" value="true" />
+  </include>
+
+  <group if="$(eval board == 'large')">
+    <arg name="squares_sides_size_in_meters" value="0.040" />
+    <arg name="markers_sides_size_in_meters" value="0.031" />
+    <arg name="number_of_squares_in_x" value="14" />
+    <arg name="number_of_squares_in_y" value="9" />
+    <arg name="number_of_markers" value="63" />
+  </group>
+
+  </include>
+  <include file="$(find charuco_detector)/launch/charuco_detector.launch"
+    if="$(eval board == 'large')" pass_all_args="true">
+    <arg name="image_topic" default="/rgbd_camera/stereo/image_raw" />
+    <arg name="camera_info_topic" default="/rgbd_camera/stereo/camera_info" />
+  </include>
+
+  <include file="$(find easy_handeye)/launch/calibrate.launch">
+    <arg name="eye_on_hand" value="true" />
+
+    <!-- you can choose any identifier, as long as you use the same for publishing the calibration -->
+    <arg name="namespace_prefix" value="my_eih_calib" />
+
+    <!-- fill in the following parameters according to your robot's published tf frames -->
+    <arg name="robot_base_frame" value="/base_link" />
+    <arg name="robot_effector_frame" value="/tool0" />
+
+    <!-- fill in the following parameters according to your tracking system's published tf frames -->
+    <arg name="tracking_base_frame" value="/rgbd_camera_rgb_camera_optical_frame" />
+    <arg name="tracking_marker_frame" value="/charuco" />
+  </include>
+</launch>
diff --git a/package.xml b/package.xml
index f4ac8c9..99bf4d1 100644
--- a/package.xml
+++ b/package.xml
@@ -31,6 +31,8 @@
   <!-- to execute xacro for abb robots -->
   <exec_depend>abb_resources</exec_depend>
   <exec_depend>octomap_server</exec_depend>
+  <exec_depend>easy_handeye</exec_depend>
+  <exec_depend>charuco_detector</exec_depend>
 
   <buildtool_depend>catkin</buildtool_depend>
   <!-- The export tag contains other, unspecified, tags -->