eduro/cube.py at master · tajgr/eduro · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
#!/usr/bin/python
"""
  Cube detector with Velodyne VLP-16
  (planned for SICK Robot Day 2016)
  usage:
       ./cube.py <task|thread> [<metalog> [<F>]]
"""
# requires source from osgar & apyros

import sys
import os
import inspect
OSGAR_ROOT = os.path.realpath(os.path.abspath(os.path.join(
    os.path.split(inspect.getfile(inspect.currentframe() ))[0], '..', 'osgar')))
if OSGAR_ROOT not in sys.path:
    assert 'eduro' in sys.path[0], sys.path
    sys.path.insert(1, OSGAR_ROOT) # access without installation
#    sys.path.append(OSGAR_ROOT)  # collision of can.py and other files
# TODO fix velodyne logging and provide apyros+sensors Python package

from velodyne import Velodyne, LASER_ANGLES
from apyros.metalog import MetaLog, disableAsserts

import numpy as np


def load_background():
#    return np.zeros((360, 16), dtype=np.uint16)  # default = no action
#    return np.full((360, 16), 1000, dtype=np.uint16)  # just for subtraction test
    return np.loadtxt('cube-background.txt', dtype=np.uint16)


def save_background(arr, filename):
    f = open(filename, 'w')
    for i in xrange(360):
        for j in xrange(16):
            f.write('{} '.format(arr[i][j]))
        f.write('\n')
    f.close()


def remove_background(scan, ground):
    mask = scan >= background
    ret = scan.copy()
    ret[mask] = 0
    return ret


def print_data(scan):
    print [x for a,x in sorted(zip(LASER_ANGLES, scan[0]))]
    print


##################### small SICK laser scanner #####################

def detect_cubes(raw_laser_data, verbose=False):
    arr = np.array(raw_laser_data)
    mask = arr < 200
    arr[mask] = 10000
    blind_offset = 45
    index = np.argmin(arr[blind_offset:]) + blind_offset
    if arr[index] < 2000:  # 2 meters
        left_index = right_index = index
        cube_max_dist = arr[index] + 110
        while left_index > 0 and arr[left_index] < cube_max_dist:
            left_index -= 1
        while right_index < len(arr) and arr[right_index] < cube_max_dist:
            right_index += 1
        center_index = (left_index + right_index)/2
        if verbose:
            print left_index, index, right_index, '->', center_index
            print arr[left_index:right_index+1]
        return [(center_index, arr[center_index])]
    return []

if __name__ == "__main__":
    if len(sys.argv) < 2:
        print __doc__
        sys.exit(2)
    metalog=None
    if 'meta_' in sys.argv[1]:
        metalog = MetaLog(filename=sys.argv[1])
    elif len(sys.argv) > 2:
        metalog = MetaLog(filename=sys.argv[2])
    if len(sys.argv) > 2 and sys.argv[-1] == 'F':
        disableAsserts()

    sensor = Velodyne(metalog=metalog)
    if sys.argv[1] == 'thread':
        thr = VelodyneThread(sensor)
        start_time = datetime.now()
        thr.start()
        prev = None
        while datetime.now() - start_time < timedelta(seconds=3.0):
            curr = thr.scan_safe_dist()
            if prev != curr:
                print curr
            prev = curr
        thr.requestStop()
        thr.join()
    else:
        background = load_background()
        prev = None
        for i in xrange(10000):
            sensor.update()
            curr = sensor.scan_index, sensor.dist_index
            if prev != curr:
                data = remove_background(sensor.dist, background)
                print_data(data)
#                if sensor.scan_index % 10 == 0:
#                    print curr
            prev = curr

# vim: expandtab sw=4 ts=4