CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-06-08 18:44:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

genmap.py: Add x0 and y0 shift for markers coordinates

Browse Source
This commit is contained in:
Arthur Golubtsov
2020-06-25 00:31:56 +03:00
parent 62a77519e6
commit b713b49f3f
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
aruco_pose/src/genmap.py
View File

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