6FPS/car-os
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Adding lidar sensor handling

Browse Source
This commit is contained in:
smithserver_admin
2026-07-08 20:58:21 -06:00
parent 8fc5088c01
commit 04f3e2ba07
19 changed files with 145 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/lidar_2d_sensor_node.gd
+17 -1
View File
@@ -1,9 +1,14 @@
extends BaseSensorNode
class_name Lidar2DSensorNode
# Called when the node enters the scene tree for the first time.
func _ready() -> void:
pass # Replace with function body.
super._ready()
DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received)
DataSignals.gps_data_received.connect(_on_gps_data_received)
DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
DataSignals.gyroscope_data_received.connect(_on_gyro_data_received)
# Called every frame. 'delta' is the elapsed time since the previous frame.
@@ -12,3 +17,14 @@ func _process(delta: float) -> void:
func _get_sensor_class():
return "LIDAR_2D"
func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):pass
func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordinates: Array, speed_kmh: float, dop: Array):
pass
func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
pass
func _on_gyro_data_received(source_ip: String, timestamp_list: Array, gyro_data: Array):
pass
sensors/vehicle_sensor_node.gd
+18
View File
@@ -48,6 +48,7 @@ var heading_rad: float
func _ready() -> void:
super._ready()
DataSignals.gps_data_received.connect(_on_gps_data_received)
#DataSignals.sat_data_received.connect(_on_sat_data_received)
DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
@@ -63,6 +64,12 @@ func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordin
if source_ip != ip_address:
return
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if gps_timestamps.size() > 0 and (timestamp - gps_timestamps[-1]) < -1:
gps_timestamps.clear()
coordinates.clear()
gps_positions.clear()
altitudes.clear()
speed_data.clear()
if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
return
var insert_idx = Bisect.reverse_bisect(gps_timestamps, timestamp)
@@ -93,6 +100,13 @@ func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Ve
if source_ip != ip_address:
return
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if accel_timestamps.size() > 0 and (timestamp - accel_timestamps[-1]) < -1:
accel_timestamps.clear()
linear_vecs.clear()
g_vecs.clear()
pitch_data.clear()
roll_data.clear()
lateral_g_data.clear()
if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp):
return
var insert_idx = Bisect.reverse_bisect(accel_timestamps, timestamp)
@@ -122,6 +136,10 @@ func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Ve
if source_ip != ip_address:
return
var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
if mag_timestamps.size() > 0 and (timestamp - mag_timestamps[-1]) < -1:
mag_timestamps.clear()
mag_vecs.clear()
heading_data.clear()
if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp):
return
var insert_idx = Bisect.reverse_bisect(mag_timestamps, timestamp)