eduro/cube.py at master · robotika/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
#!/usr/bin/python
"""
  Cube detector with small horizontally placed SICK laser rangefinder
  (planned for SICK Robot Day 2016)
  usage:
       ./cube.py <src_laser file>
"""
import sys
import numpy as np
import math

def detect_cubes_v0(raw_laser_data, verbose=False):
    arr = np.array(raw_laser_data)
    mask = arr < 10  # 1cm "blindnesss"
    arr[mask] = 10000
    blind_offset = 45
    index = np.argmin(arr[blind_offset:]) + blind_offset
    while 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
        cube_size = math.radians(right_index - left_index) * arr[center_index]/1000.0

        # diagonal distance of the first and the last point
        # using Cosine theorem
        gama = math.radians(right_index - left_index)
        a = arr[left_index + 1]/1000.0
        b = arr[right_index - 1]/1000.0
        diagonal_size = math.sqrt(abs(a*a + b*b - 2*a*b*math.cos(gama)))
        if verbose:
            print left_index, index, right_index, '->', center_index
            print arr[left_index:right_index+1]
            print "CUBE SIZE", cube_size, diagonal_size
        if 0.1 < cube_size < 0.25 or 0.1 < diagonal_size < 0.25:
            if (0.1 < cube_size < 0.25) != (0.1 < diagonal_size < 0.25):
                print "Cube Size WARNING", cube_size, diagonal_size
            yield (center_index, arr[center_index])
        arr[left_index + 1:right_index] = 10000
        index = np.argmin(arr[blind_offset:]) + blind_offset


def verify_loaded_cube(data):
    print 'verify_loaded_cube'
    loaded = min(data[:45]) < 100
    print data[:135:10]
    return loaded, 0


class CubeDetector:

    def __init__(self, laser_pose_6D, verbose=False):
        self.laser_x = laser_pose_6D[0][0]
        self.laser_y = laser_pose_6D[0][1]
        self.verbose = verbose

    def detect_cubes_xy(self, raw_laser_data, limit=3):
        """return list of cubes coordinates relative to robot position"""
        ret = []
        for deg_angle, mm_dist in detect_cubes_v0(raw_laser_data, verbose=self.verbose):
             angle, dist = math.radians(135-deg_angle), mm_dist/1000.0
             cube_x, cube_y = self.laser_x + math.cos(angle)*dist, self.laser_y + math.sin(angle)*dist
             ret.append((cube_x, cube_y))
             if self.verbose:
                 print "{:.2f}\t{:.2f}".format(cube_x, cube_y)
             if len(ret) >= limit:
                 break
        return ret


if __name__ == "__main__":
    if len(sys.argv) < 2:
        print __doc__
        sys.exit(2)

    cd = CubeDetector(((0.24, -0.13, 0.08), (0,math.radians(180),0)))
    for i, line in enumerate(open(sys.argv[1]), start=1):
        if '[' in line:
            laser_data = eval(line)
            cubes = cd.detect_cubes_xy(laser_data)
            if len(cubes) == 0:
                print i
                cd.detect_cubes_xy(laser_data, verbose=True)


# vim: expandtab sw=4 ts=4