40 lines
1.1 KiB
GDScript
40 lines
1.1 KiB
GDScript
|
extends Node2D
|
||
|
|
|
||
|
|
var line: Line2D
|
||
|
|
|
||
|
|
var points_angle = []
|
||
|
|
var points_distance = []
|
||
|
|
var timer_static = 60
|
||
|
|
var timer = 0.
|
||
|
|
|
||
|
|
# Called when the node enters the scene tree for the first time.
|
||
|
|
func _ready() -> void:
|
||
|
|
line = get_node('Line2D')
|
||
|
|
DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received)
|
||
|
|
|
||
|
|
|
||
|
|
# Called every frame. 'delta' is the elapsed time since the previous frame.
|
||
|
|
func _process(delta: float) -> void:
|
||
|
|
timer += delta
|
||
|
|
if timer > timer_static:
|
||
|
|
timer = 0
|
||
|
|
points_angle.clear()
|
||
|
|
points_distance.clear()
|
||
|
|
for i in line.points:
|
||
|
|
print(i - Vector2(400, 400))
|
||
|
|
print()
|
||
|
|
_update_line()
|
||
|
|
|
||
|
|
func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):
|
||
|
|
for point in lidar_data:
|
||
|
|
var insert_idx = Bisect.bisect(points_angle, point[0])
|
||
|
|
points_angle.insert(insert_idx, point[0])
|
||
|
|
points_distance.insert(insert_idx, point[1])
|
||
|
|
|
||
|
|
func _update_line():
|
||
|
|
line.clear_points()
|
||
|
|
for idx in range(points_angle.size()):
|
||
|
|
var x = 4*points_distance[idx] * cos(deg_to_rad(points_angle[idx])) + 400
|
||
|
|
var y = 4*points_distance[idx] * sin(deg_to_rad(points_angle[idx])) + 400
|
||
|
|
line.add_point(Vector2(x, y))
|