Skip to content

gui

LidarObstacleTracker #

Source code in cogip/tools/ydlidar_g2/gui.py 
 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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
class LidarObstacleTracker:
    def __init__(
        self,
        lidar_coords: NDArray,
        lidar_offset: tuple[float, float],
        eps: float = 30.0,
        min_samples: int = 6,
        update_interval: int = 100,
    ):
        """
        Initialize the real-time Lidar obstacle tracker

        Args:
            lidar_coords: 2D NDArray with shape (MAX_LIDAR_DATA_COUNT, 2) containing x and y global coordinates
            lidar_offset: Lidar offset from robot center
            eps: DBSCAN clustering parameter
            min_samples: Minimum points for cluster formation
            update_interval: Visualization update interval
        """
        # Use default pose if not provided
        self.lidar_coords = lidar_coords
        self.lidar_offset = lidar_offset
        self.eps = eps
        self.min_samples = min_samples
        self.update_interval = update_interval
        self.view_radius = 2500
        self.clusters: list[NDArray] = []
        self.obstacle_properties: list[tuple[float, float, float, float]] = []

        # Initialize plot and data containers
        self.fig, self.ax = plt.subplots(figsize=(10, 10))
        self.setup_plot()

        # Connect the scroll event to the handler
        self.fig.canvas.mpl_connect("scroll_event", self.on_scroll)

        # Visualization elements
        self.points_scatter = self.ax.scatter([], [], c="gray", s=5, label="Detected Points")
        self.cluster_scatters: list[PathCollection] = []
        self.obstacle_circles: list[Ellipse] = []

        # Robot and Lidar markers
        self.robot_marker = self.ax.scatter(
            0,
            0,
            c="red",
            s=100,
            marker="*",
            label="Robot",
        )

        # Calculate Lidar position
        self.lidar_marker = self.ax.scatter(
            self.lidar_offset[1],
            self.lidar_offset[0],
            c="blue",
            s=80,
            marker="o",
            label="Lidar",
        )

        # Animation setup
        self.animation: FuncAnimation | None = None

    def setup_plot(self):
        """Configure the plot appearance with dark theme"""
        # Set figure and axes background color
        self.fig.patch.set_facecolor("#2E2E2E")
        self.ax.set_facecolor("#1E1E1E")

        # Set labels and title with light colors
        self.ax.set_xlabel("Y (mm)", color="#CCCCCC")
        self.ax.set_ylabel("X (mm)", color="#CCCCCC")
        self.ax.set_title("Real-time Obstacle Detection", color="#FFFFFF", fontweight="bold")

        # Customize grid
        self.ax.grid(True, color="#555555", linestyle="-", linewidth=0.5, alpha=0.7)

        # Customize axis appearance
        self.ax.spines["bottom"].set_color("#555555")
        self.ax.spines["top"].set_color("#555555")
        self.ax.spines["left"].set_color("#555555")
        self.ax.spines["right"].set_color("#555555")

        # Customize tick parameters
        self.ax.tick_params(axis="both", colors="#CCCCCC")

        # Invert x-axis and set equal aspect ratio
        self.ax.invert_xaxis()
        self.ax.axis("equal")

        # Configure legend with dark theme colors
        self.ax.legend(facecolor="#333333", edgecolor="#555555", labelcolor="#CCCCCC")

        # Set initial view range
        self.ax.set_xlim((self.view_radius, -self.view_radius))
        self.ax.set_ylim((-self.view_radius, self.view_radius))

    def cluster_obstacles(self, points: NDArray) -> list[NDArray]:
        """
        Groups points into obstacle clusters using DBSCAN

        Args:
            points: NDArray of (x, y) points representing detected obstacles

        Returns:
            List of clusters, each cluster being a set of points belonging to the same obstacle
        """
        if len(points) == 0:
            return []

        db = DBSCAN(eps=self.eps, min_samples=self.min_samples).fit(points)
        labels = db.labels_

        n_clusters = len(set(labels)) - (1 if -1 in labels else 0)

        clusters = []
        for i in range(n_clusters):
            cluster_points = points[labels == i]
            clusters.append(cluster_points)

        return clusters

    def estimate_obstacle_properties(self, clusters: list[NDArray]) -> list[tuple[float, float, float]]:
        """
        Estimates position and size of obstacles from clusters

        Args:
            clusters: List of clusters, each cluster being a set of points

        Returns:
            List of tuples (center_x, center_y, radius) for each obstacle
        """
        obstacle_properties = []

        for cluster in clusters:
            center_x = np.mean(cluster[:, 0])
            center_y = np.mean(cluster[:, 1])

            # Calculate the maximum distance from center in x and y directions
            # This will be used as the radius of the circle
            radius_x = np.max(np.abs(cluster[:, 0] - center_x))
            radius_y = np.max(np.abs(cluster[:, 1] - center_y))
            radius = max(radius_x, radius_y, 20)  # Minimum radius of 20

            obstacle_properties.append((center_x, center_y, radius))

        return obstacle_properties

    def update_plot(self, frame):
        """Updates the visualization with current data"""
        lidar_coords = self.lidar_coords[: np.argmax(self.lidar_coords[:, 0] == -1)].copy()
        self.clusters = self.cluster_obstacles(lidar_coords)
        self.obstacle_properties = self.estimate_obstacle_properties(self.clusters)

        # Update points scatter
        self.points_scatter.set_offsets(np.column_stack((lidar_coords[:, 1], lidar_coords[:, 0])))

        # Clear previous cluster scatters and obstacle visualizations
        for scatter in self.cluster_scatters:
            scatter.remove()
        self.cluster_scatters = []

        for circle in self.obstacle_circles:
            circle.remove()
        self.obstacle_circles = []

        # Create color map for clusters that works well with dark theme
        colors = plt.cm.plasma(np.linspace(0, 1, max(1, len(self.clusters))))

        # Draw new clusters
        for i, cluster in enumerate(self.clusters):
            scatter = self.ax.scatter(
                cluster[:, 1],
                cluster[:, 0],
                c=[colors[i]],
                s=20,
                label=f"Cluster {i}" if i == 0 else "",
            )
            self.cluster_scatters.append(scatter)

        # Draw obstacle circles and labels
        for i, (center_x, center_y, radius) in enumerate(self.obstacle_properties):
            # Create ellipse for the obstacle
            circle = Ellipse(
                (center_y, center_x),
                width=radius * 2,
                height=radius * 2,
                fill=False,
                edgecolor=colors[i],
                linewidth=2,
                alpha=0.8,
            )
            self.ax.add_patch(circle)
            self.obstacle_circles.append(circle)

        # Redraw the figure
        self.fig.canvas.draw_idle()

    def start_animation(self):
        """Starts the real-time visualization"""
        # Set dark theme for the color map (for clusters)
        plt.rcParams["axes.prop_cycle"] = plt.cycler(color=plt.cm.plasma(np.linspace(0, 1, 10)))

        # Continue with original animation code
        self.animation = FuncAnimation(
            self.fig,
            self.update_plot,
            interval=self.update_interval,
            blit=False,
            cache_frame_data=False,
        )

    def on_scroll(self, event: MouseEvent):
        # Ignore if the mouse is not over the axes
        if event.inaxes != self.ax:
            return

        # Get the current limits
        xlim = self.ax.get_xlim()
        ylim = self.ax.get_ylim()

        # Get mouse position in data coordinates
        x_data, y_data = event.xdata, event.ydata

        # Calculate zoom factor
        zoom_factor = 1.1 if event.button == "down" else 0.9  # Zoom in/out

        # Calculate new limits maintaining the mouse position as center
        x_left = x_data - zoom_factor * (x_data - xlim[0])
        x_right = x_data + zoom_factor * (xlim[1] - x_data)
        y_bottom = y_data - zoom_factor * (y_data - ylim[0])
        y_top = y_data + zoom_factor * (ylim[1] - y_data)

        # Limit the zoom range
        x_left = max(-self.view_radius, x_left)
        x_right = min(self.view_radius, x_right)
        y_bottom = max(-self.view_radius, y_bottom)
        y_top = min(self.view_radius, y_top)

        # Apply the new limits
        self.ax.set_xlim(x_left, x_right)
        self.ax.set_ylim(y_bottom, y_top)

        # Redraw the plot
        plt.draw()

__init__(lidar_coords, lidar_offset, eps=30.0, min_samples=6, update_interval=100) #

Initialize the real-time Lidar obstacle tracker

Parameters:

Name Type Description Default
lidar_coords NDArray

2D NDArray with shape (MAX_LIDAR_DATA_COUNT, 2) containing x and y global coordinates

 required
lidar_offset tuple[float, float]

Lidar offset from robot center

 required
eps float

DBSCAN clustering parameter

 30.0
min_samples int

Minimum points for cluster formation

 6
update_interval int

Visualization update interval

 100
Source code in cogip/tools/ydlidar_g2/gui.py 
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
def __init__(
    self,
    lidar_coords: NDArray,
    lidar_offset: tuple[float, float],
    eps: float = 30.0,
    min_samples: int = 6,
    update_interval: int = 100,
):
    """
    Initialize the real-time Lidar obstacle tracker

    Args:
        lidar_coords: 2D NDArray with shape (MAX_LIDAR_DATA_COUNT, 2) containing x and y global coordinates
        lidar_offset: Lidar offset from robot center
        eps: DBSCAN clustering parameter
        min_samples: Minimum points for cluster formation
        update_interval: Visualization update interval
    """
    # Use default pose if not provided
    self.lidar_coords = lidar_coords
    self.lidar_offset = lidar_offset
    self.eps = eps
    self.min_samples = min_samples
    self.update_interval = update_interval
    self.view_radius = 2500
    self.clusters: list[NDArray] = []
    self.obstacle_properties: list[tuple[float, float, float, float]] = []

    # Initialize plot and data containers
    self.fig, self.ax = plt.subplots(figsize=(10, 10))
    self.setup_plot()

    # Connect the scroll event to the handler
    self.fig.canvas.mpl_connect("scroll_event", self.on_scroll)

    # Visualization elements
    self.points_scatter = self.ax.scatter([], [], c="gray", s=5, label="Detected Points")
    self.cluster_scatters: list[PathCollection] = []
    self.obstacle_circles: list[Ellipse] = []

    # Robot and Lidar markers
    self.robot_marker = self.ax.scatter(
        0,
        0,
        c="red",
        s=100,
        marker="*",
        label="Robot",
    )

    # Calculate Lidar position
    self.lidar_marker = self.ax.scatter(
        self.lidar_offset[1],
        self.lidar_offset[0],
        c="blue",
        s=80,
        marker="o",
        label="Lidar",
    )

    # Animation setup
    self.animation: FuncAnimation | None = None

cluster_obstacles(points) #

Groups points into obstacle clusters using DBSCAN

Parameters:

Name Type Description Default
points NDArray

NDArray of (x, y) points representing detected obstacles

 required

Returns:

Type Description
list[NDArray]

List of clusters, each cluster being a set of points belonging to the same obstacle
Source code in cogip/tools/ydlidar_g2/gui.py 
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
def cluster_obstacles(self, points: NDArray) -> list[NDArray]:
    """
    Groups points into obstacle clusters using DBSCAN

    Args:
        points: NDArray of (x, y) points representing detected obstacles

    Returns:
        List of clusters, each cluster being a set of points belonging to the same obstacle
    """
    if len(points) == 0:
        return []

    db = DBSCAN(eps=self.eps, min_samples=self.min_samples).fit(points)
    labels = db.labels_

    n_clusters = len(set(labels)) - (1 if -1 in labels else 0)

    clusters = []
    for i in range(n_clusters):
        cluster_points = points[labels == i]
        clusters.append(cluster_points)

    return clusters

estimate_obstacle_properties(clusters) #

Estimates position and size of obstacles from clusters

Parameters:

Name Type Description Default
clusters list[NDArray]

List of clusters, each cluster being a set of points

 required

Returns:

Type Description
list[tuple[float, float, float]]

List of tuples (center_x, center_y, radius) for each obstacle
Source code in cogip/tools/ydlidar_g2/gui.py 
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
def estimate_obstacle_properties(self, clusters: list[NDArray]) -> list[tuple[float, float, float]]:
    """
    Estimates position and size of obstacles from clusters

    Args:
        clusters: List of clusters, each cluster being a set of points

    Returns:
        List of tuples (center_x, center_y, radius) for each obstacle
    """
    obstacle_properties = []

    for cluster in clusters:
        center_x = np.mean(cluster[:, 0])
        center_y = np.mean(cluster[:, 1])

        # Calculate the maximum distance from center in x and y directions
        # This will be used as the radius of the circle
        radius_x = np.max(np.abs(cluster[:, 0] - center_x))
        radius_y = np.max(np.abs(cluster[:, 1] - center_y))
        radius = max(radius_x, radius_y, 20)  # Minimum radius of 20

        obstacle_properties.append((center_x, center_y, radius))

    return obstacle_properties

setup_plot() #

Configure the plot appearance with dark theme

Source code in cogip/tools/ydlidar_g2/gui.py 
 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
def setup_plot(self):
    """Configure the plot appearance with dark theme"""
    # Set figure and axes background color
    self.fig.patch.set_facecolor("#2E2E2E")
    self.ax.set_facecolor("#1E1E1E")

    # Set labels and title with light colors
    self.ax.set_xlabel("Y (mm)", color="#CCCCCC")
    self.ax.set_ylabel("X (mm)", color="#CCCCCC")
    self.ax.set_title("Real-time Obstacle Detection", color="#FFFFFF", fontweight="bold")

    # Customize grid
    self.ax.grid(True, color="#555555", linestyle="-", linewidth=0.5, alpha=0.7)

    # Customize axis appearance
    self.ax.spines["bottom"].set_color("#555555")
    self.ax.spines["top"].set_color("#555555")
    self.ax.spines["left"].set_color("#555555")
    self.ax.spines["right"].set_color("#555555")

    # Customize tick parameters
    self.ax.tick_params(axis="both", colors="#CCCCCC")

    # Invert x-axis and set equal aspect ratio
    self.ax.invert_xaxis()
    self.ax.axis("equal")

    # Configure legend with dark theme colors
    self.ax.legend(facecolor="#333333", edgecolor="#555555", labelcolor="#CCCCCC")

    # Set initial view range
    self.ax.set_xlim((self.view_radius, -self.view_radius))
    self.ax.set_ylim((-self.view_radius, self.view_radius))

start_animation() #

Starts the real-time visualization

Source code in cogip/tools/ydlidar_g2/gui.py 
210
211
212
213
214
215
216
217
218
219
220
221
222
def start_animation(self):
    """Starts the real-time visualization"""
    # Set dark theme for the color map (for clusters)
    plt.rcParams["axes.prop_cycle"] = plt.cycler(color=plt.cm.plasma(np.linspace(0, 1, 10)))

    # Continue with original animation code
    self.animation = FuncAnimation(
        self.fig,
        self.update_plot,
        interval=self.update_interval,
        blit=False,
        cache_frame_data=False,
    )

update_plot(frame) #

Updates the visualization with current data

Source code in cogip/tools/ydlidar_g2/gui.py 
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
def update_plot(self, frame):
    """Updates the visualization with current data"""
    lidar_coords = self.lidar_coords[: np.argmax(self.lidar_coords[:, 0] == -1)].copy()
    self.clusters = self.cluster_obstacles(lidar_coords)
    self.obstacle_properties = self.estimate_obstacle_properties(self.clusters)

    # Update points scatter
    self.points_scatter.set_offsets(np.column_stack((lidar_coords[:, 1], lidar_coords[:, 0])))

    # Clear previous cluster scatters and obstacle visualizations
    for scatter in self.cluster_scatters:
        scatter.remove()
    self.cluster_scatters = []

    for circle in self.obstacle_circles:
        circle.remove()
    self.obstacle_circles = []

    # Create color map for clusters that works well with dark theme
    colors = plt.cm.plasma(np.linspace(0, 1, max(1, len(self.clusters))))

    # Draw new clusters
    for i, cluster in enumerate(self.clusters):
        scatter = self.ax.scatter(
            cluster[:, 1],
            cluster[:, 0],
            c=[colors[i]],
            s=20,
            label=f"Cluster {i}" if i == 0 else "",
        )
        self.cluster_scatters.append(scatter)

    # Draw obstacle circles and labels
    for i, (center_x, center_y, radius) in enumerate(self.obstacle_properties):
        # Create ellipse for the obstacle
        circle = Ellipse(
            (center_y, center_x),
            width=radius * 2,
            height=radius * 2,
            fill=False,
            edgecolor=colors[i],
            linewidth=2,
            alpha=0.8,
        )
        self.ax.add_patch(circle)
        self.obstacle_circles.append(circle)

    # Redraw the figure
    self.fig.canvas.draw_idle()