[jsk_pr2_accessories] Generate april-tag image and plate stl file for hand calibration

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/.gitignore

@@ -0,0 +1,7 @@
+/build
+*.stl
+*.pdf
+*.out
+*.log
+*.aux
+*.dvi

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/Makefile

@@ -0,0 +1,32 @@
+OPENSCAD_CHECK := $(shell command -v openscad 2> /dev/null)
+PDFLATEX_CHECK := $(shell command -v pdflatex 2> /dev/null)
+
+all: check-deps build/tag_plate.stl build/april_tag_print.pdf
+
+check-deps:
+ifndef OPENSCAD_CHECK
+	$(error "OpenSCAD is required. Please install: sudo apt install openscad")
+endif
+ifndef PDFLATEX_CHECK
+	$(error "pdflatex is required. Please install: sudo apt install texlive-base")
+endif
+
+build/tag_plate.stl: tag_plate.scad
+	mkdir -p build
+	openscad -o $@ $<
+	echo "Generated $@"
+
+build/tag_with_margin.png: rotate_and_add_margin.py
+	mkdir -p build
+	python3 ./rotate_and_add_margin.py tag41_12_00000.png $@
+	echo "Generated $@"
+
+build/april_tag_print.pdf: april_tag_print.tex build/tag_with_margin.png
+	mkdir -p build
+	pdflatex -output-directory=build $<
+	echo "Generated $@"
+
+clean:
+	rm -rf build
+
+.PHONY: all clean check-deps

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/README.md

@@ -0,0 +1,28 @@
+## Motivation
+PR2's calibrations (intrinsics/extrinsics/kinematics) are very challenging.
+A simple but effective workaround is to attach markers to the PR2's gripper and compare the pose calculated by nominal kinematics with the detected pose.
+We have been using yellow tape as markers, but it's difficult to maintain their position and orientation consistently.
+Therefore, we decided to create a 3D-printed tag plate for the PR2's gripper.
+
+## Requirements
+Software:
+```bash
+sudo apt install texlive-base openscad
+```
+Hardware:
+- スペーサM3オスメス六角10mm (x2) (Recommended to use plastic spacers to not damage the aluminum lid)
+- 六角穴付きボルトM3-10mm (x2)
+
+## Assebly instructions
+- Then run `make` command in this directory.
+- Print the generated PDF file under `build/april_tag_print.pdf`
+- 3D print the generated STL file under `build/tag_plate.stl`
+- Remove the two bolts from the lid of PR2's gripper's back side. Don't remove the lid.
+- Attach the printed tag plate to the lid using the two bolts with the spacers.
+- Paste the printed tag on the tag plate. The printed tag size is supposed to be exactly equal to the tag plate size.
+
+See the images under [./docs_image/](docs_image/) for the reference.
+
+## Example usage
+- launch `example/kinect_april_tag_detection.launch` inside PR2's machine.
+- Check tf tree to see `april` frame.

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/april_tag_print.tex

@@ -0,0 +1,19 @@
+\documentclass{article}
+\usepackage{graphicx}
+\usepackage[margin=1cm]{geometry}
+\usepackage{subfigure}
+\usepackage{xcolor}
+
+\begin{document}
+
+\setlength{\fboxsep}{0pt}
+\setlength{\fboxrule}{0.5pt}
+\definecolor{lightgray}{gray}{0.8}
+
+\begin{figure}[htbp]
+    \centering
+    \fcolorbox{lightgray}{white}{\includegraphics[scale=1.685]{./build/tag_with_margin.png}}
+    \caption{5cm x 5cm AprilTags with white margin. Please print on A4 paper and cut along the gray lines.}
+\end{figure}
+
+\end{document}

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/example/kinect_april_tag_detection.launch

@@ -0,0 +1,18 @@
+<launch>
+  <!-- You need to run this launch file in the PR2 robot for higher frequency of the image,
+  otherwise the internal message filter will not work -->
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_pose_detector">
+    <remap from="image_rect" to="/kinect_head/rgb/image_rect_color" />
+    <remap from="camera_info" to="/kinect_head/rgb/camera_info" />
+    <rosparam>
+      <!-- NOTE: the size is the inner square's edge length of the tag, not the outer edge length!!! -->
+      standalone_tags: [{id: 0, size: 0.0277, name: apriltag},]
+      tag_family: "tagStandard41h12"
+      publish_tf: true
+    </rosparam>
+  </node>
+
+  <!-- NOTE: -0.04 is measured by hand. -.12 is determined by "eye calibration" -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="sensor_frame_publisher"
+        args="-0.12 0 -0.04 0 0 3.1415926 l_gripper_tool_frame apriltag_fk" />
+</launch>

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/rotate_and_add_margin.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+
+from PIL import Image
+import argparse
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument('input', help='input image path')
+    parser.add_argument('output', help='output image path')
+    args = parser.parse_args()
+
+    img = Image.open(args.input)
+    img = img.rotate(90)
+
+    if img.width != img.height:
+        raise ValueError("Input image must be square")
+
+    # increas the resolution of image by 10 times because the original image is too small!
+    img = img.resize((img.width * 10, img.width * 10), Image.NEAREST)
+
+    w = img.width
+    w_B = int(w * (63 / 50))
+    h_B = int(w * (55 / 50))
+    margin_left = (w_B - w) // 2
+    margin_top = (h_B - w) // 2
+
+    new_img = Image.new('RGB', (w_B, h_B), (255, 255, 255))
+    new_img.paste(img, (margin_left, margin_top))
+    new_img.save(args.output)
+
+if __name__ == '__main__':
+    main()

jsk_pr2_robot/jsk_pr2_accessories/pr2_hand_apriltag/tag_plate.scad

@@ -0,0 +1,28 @@
+board_width = 63;
+board_length = 55;
+board_height = 6.0;
+hole_diameter = 3.2;
+screw_head_diameter_with_margin = 6.3;
+screw_head_depth_with_margin = 3.2;
+
+hole_ypos = 15;
+hole_interval = 50;
+hole1_pos = [(board_width - hole_interval) * 0.5, hole_ypos];
+hole2_pos = [(board_width + hole_interval) * 0.5, hole_ypos];
+
+difference() {
+    cube([board_width, board_length, board_height]);
+    translate([hole1_pos[0], hole1_pos[1], 0]){
+        cylinder(h = board_height, d = hole_diameter, $fn = 32);
+    }
+    translate([hole2_pos[0], hole2_pos[1], 0]){
+        cylinder(h = board_height, d = hole_diameter, $fn = 32);
+    }
+    translate([hole1_pos[0], hole1_pos[1], 0]){
+        cylinder(h = screw_head_depth_with_margin, d = screw_head_diameter_with_margin, $fn = 32);
+    }
+    translate([hole2_pos[0], hole2_pos[1], 0]){
+        cylinder(h = screw_head_depth_with_margin, d = screw_head_diameter_with_margin, $fn = 32);
+    }
+
+}