From b713b49f3f5b4d7ade2edc23bfbbf48cabc05cb2 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Thu, 25 Jun 2020 00:31:56 +0300
Subject: [PATCH] genmap.py: Add x0 and y0 shift for markers coordinates

---
 aruco_pose/src/genmap.py | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/aruco_pose/src/genmap.py b/aruco_pose/src/genmap.py
index 0050fc4f..a82b52b8 100755
--- a/aruco_pose/src/genmap.py
+++ b/aruco_pose/src/genmap.py
@@ -13,7 +13,7 @@
 Generate map file for aruco_map nodelet.
 
 Usage:
-  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [--top-left | --bottom-left]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left]
   genmap.py (-h | --help)
 
 Options:
@@ -23,6 +23,8 @@ Options:
   <dist_x>       Distance between markers along X axis
   <dist_y>       Distance between markers along Y axis
   <first>        First marker ID [default: 0]
+  <x0>           X coordinate for the first marker [default: 0]
+  <y0>           Y coordinate for the first marker [default: 0]
   --top-left     First marker is on top-left (default)
   --bottom-left  First marker is on bottom-left
 
@@ -39,20 +41,22 @@ arguments = docopt(__doc__)
 
 length = float(arguments['<length>'])
 first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
+x0 = float(arguments['<x0>'] if arguments['<x0>'] is not None else 0)
+y0 = float(arguments['<y0>'] if arguments['<y0>'] is not None else 0)
 markers_x = int(arguments['<x>'])
 markers_y = int(arguments['<y>'])
 dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']
 
-max_y = (markers_y - 1) * dist_y
+max_y = y0 + (markers_y - 1) * dist_y
 
 print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
 for y in range(markers_y):
     for x in range(markers_x):
-        pos_x = x * dist_x
-        pos_y = y * dist_y
+        pos_x = x0 + x * dist_x
+        pos_y = y0 + y * dist_y
         if not bottom_left:
             pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
         first += 1