AutonomousVehicleControlBeginnersGuide/src/components/sensors/lidar/scan_point.py at main · ShisatoYano/AutonomousVehicleControlBeginnersGuide · 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
"""
scan_point.py

Author: Shisato Yano
"""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../../array")
sys.path.append(str(Path(__file__).absolute().parent) + "/../../common")
from xy_array import XYArray
from matrix_lib import hom_mat_33


class ScanPoint:
    """
    Scan point of sensor class includes each sensing data
    """

    def __init__(self, distance_m, angle_rad, x_m, y_m):
        """
        Constructor
        distance_m: sensed distance data[m]
        angle_rad: sensed angle data[rad]
        x_m: sensed point's x coordinate data[m]
        y_m: sensed point's y coordinate data[m]
        """

        self.distance_m = distance_m
        self.angle_rad = angle_rad
        self.point_array = XYArray(np.array([[x_m], [y_m]]))
        self.transformed_x = None
        self.transformed_y = None

    def get_dimension(self):
        """
        Return point's x-y array data's dimension value
        """

        return self.point_array.get_dimension()

    def get_distance_m(self):
        """
        Return point's distance data[m]
        """

        return self.distance_m

    def get_point_array(self):
        """
        Return point's x-y array data
        Type is ndarray object
        """

        return self.point_array.get_data()

    def get_transformed_data(self, sensor_lon, sensor_lat, sensor_yaw,
                             vehicle_x, vehicle_y, vehicle_yaw):
        """
        Return transformed x-y array data based on specific coordinate system
        Type is ndarray object
        sensor_lon: longitudinal position of sensor on vehicle coordinate[m]
        sensor_lat: lateral position of sensor on vehicle coordinate[m]
        sensor_yaw: yaw angle of sensor on vehicle coordinate[rad]
        vehicle_x: x position of vehicle on global coordinate[m]
        vehicle_y: y position of vehicle on global coordinate[m]
        vehicle_yaw: yaw angle of vehicle on global coordinate[rad]
        """

        # transformation matrix on sensor coordinate
        point_xy = self.point_array.get_data()
        sensor_tf = hom_mat_33(point_xy[0, 0], point_xy[1, 0], 0.0)

        # transformation matrix on vehicle coordinate
        vehicle_tf = hom_mat_33(sensor_lon, sensor_lat, sensor_yaw)

        # transformation matrix on global coordinate
        global_tf = hom_mat_33(vehicle_x, vehicle_y, vehicle_yaw)

        # homegeneous transformation from sensor to global coordinate
        transformed_points_matrix = global_tf @ vehicle_tf @ sensor_tf

        return transformed_points_matrix[0, 2], transformed_points_matrix[1, 2]

    def calculate_transformed_point(self, sensor_lon, sensor_lat, sensor_yaw,
                                    vehicle_x, vehicle_y, vehicle_yaw):
        """
        Function to calculate transformed x-y point based on specific coordinate system
        sensor_lon: longitudinal position of sensor on vehicle coordinate[m]
        sensor_lat: lateral position of sensor on vehicle coordinate[m]
        sensor_yaw: yaw angle of sensor on vehicle coordinate[rad]
        vehicle_x: x position of vehicle on global coordinate[m]
        vehicle_y: y position of vehicle on global coordinate[m]
        vehicle_yaw: yaw angle of vehicle on global coordinate[rad]
        """

        self.transformed_x, self.transformed_y = \
            self.get_transformed_data(sensor_lon, sensor_lat, sensor_yaw,
                                      vehicle_x, vehicle_y, vehicle_yaw)

    def draw(self, axes, elems):
        """
        Function to draw scan point's x-y coordinate data
        axes: Axes object of figure
        elems: List of plot objects
        """

        if self.transformed_x and self.transformed_y:
            point_plot, = axes.plot(self.transformed_x, self.transformed_y, marker='.', color='b')
            elems.append(point_plot)