3d路面合并
This commit is contained in:
parent
f27abbd0db
commit
545a8a7a60
@ -1,9 +1,10 @@
|
|||||||
[gd_scene load_steps=19 format=3 uid="uid://cxqxr0p1wkkdy"]
|
[gd_scene load_steps=23 format=3 uid="uid://cxqxr0p1wkkdy"]
|
||||||
|
|
||||||
[ext_resource type="Texture2D" uid="uid://blakv4vhyfyro" path="res://pad_hmi_ui/home/home page_bg.png" id="1_brgxr"]
|
[ext_resource type="Texture2D" uid="uid://blakv4vhyfyro" path="res://pad_hmi_ui/home/home page_bg.png" id="1_brgxr"]
|
||||||
[ext_resource type="PackedScene" uid="uid://bo8w4npgct57n" path="res://res3D/ego_vehicle.tscn" id="13_tjeco"]
|
[ext_resource type="PackedScene" uid="uid://bo8w4npgct57n" path="res://res3D/ego_vehicle.tscn" id="13_tjeco"]
|
||||||
[ext_resource type="Texture2D" uid="uid://cj40hol02w7c5" path="res://pad_hmi_ui/home/power_btn.png" id="13_yxy1g"]
|
[ext_resource type="Texture2D" uid="uid://cj40hol02w7c5" path="res://pad_hmi_ui/home/power_btn.png" id="13_yxy1g"]
|
||||||
[ext_resource type="Environment" uid="uid://c5ksuke04sr07" path="res://res3D/mainEnvironment.tres" id="14_ndify"]
|
[ext_resource type="Environment" uid="uid://c5ksuke04sr07" path="res://res3D/mainEnvironment.tres" id="14_ndify"]
|
||||||
|
[ext_resource type="Script" path="res://scripts/road_surface_mesh.gd" id="15_83dvh"]
|
||||||
[ext_resource type="PackedScene" uid="uid://dx5owafvbqdec" path="res://Modules/data_panel.tscn" id="47_jqspi"]
|
[ext_resource type="PackedScene" uid="uid://dx5owafvbqdec" path="res://Modules/data_panel.tscn" id="47_jqspi"]
|
||||||
[ext_resource type="Script" path="res://code/main.gd" id="48_l81l5"]
|
[ext_resource type="Script" path="res://code/main.gd" id="48_l81l5"]
|
||||||
[ext_resource type="PackedScene" uid="uid://tqrf0pcv1bay" path="res://Modules/main_3d_vehicle.tscn" id="48_p5kic"]
|
[ext_resource type="PackedScene" uid="uid://tqrf0pcv1bay" path="res://Modules/main_3d_vehicle.tscn" id="48_p5kic"]
|
||||||
@ -43,6 +44,12 @@ corner_radius_top_right = 1024
|
|||||||
corner_radius_bottom_right = 1024
|
corner_radius_bottom_right = 1024
|
||||||
corner_radius_bottom_left = 1024
|
corner_radius_bottom_left = 1024
|
||||||
|
|
||||||
|
[sub_resource type="ArrayMesh" id="ArrayMesh_wx0gy"]
|
||||||
|
|
||||||
|
[sub_resource type="ArrayMesh" id="ArrayMesh_n5r1a"]
|
||||||
|
|
||||||
|
[sub_resource type="ConcavePolygonShape3D" id="ConcavePolygonShape3D_t1jb5"]
|
||||||
|
|
||||||
[node name="Control" type="Control"]
|
[node name="Control" type="Control"]
|
||||||
layout_mode = 3
|
layout_mode = 3
|
||||||
anchors_preset = 15
|
anchors_preset = 15
|
||||||
@ -223,5 +230,36 @@ mouse_filter = 2
|
|||||||
[node name="WorldEnvironment" type="WorldEnvironment" parent="."]
|
[node name="WorldEnvironment" type="WorldEnvironment" parent="."]
|
||||||
environment = ExtResource("14_ndify")
|
environment = ExtResource("14_ndify")
|
||||||
|
|
||||||
|
[node name="RoadSurfaceMesh" type="Node3D" parent="."]
|
||||||
|
script = ExtResource("15_83dvh")
|
||||||
|
|
||||||
|
[node name="Traiectory" type="MeshInstance3D" parent="RoadSurfaceMesh"]
|
||||||
|
|
||||||
|
[node name="ParkingSpace" type="Node3D" parent="RoadSurfaceMesh"]
|
||||||
|
unique_name_in_owner = true
|
||||||
|
transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.01, 0)
|
||||||
|
|
||||||
|
[node name="RoadSurface" type="MeshInstance3D" parent="RoadSurfaceMesh"]
|
||||||
|
|
||||||
|
[node name="DynamicObject" type="Node3D" parent="RoadSurfaceMesh"]
|
||||||
|
|
||||||
|
[node name="left_line" type="MeshInstance3D" parent="RoadSurfaceMesh"]
|
||||||
|
unique_name_in_owner = true
|
||||||
|
transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.05, 0)
|
||||||
|
mesh = SubResource("ArrayMesh_wx0gy")
|
||||||
|
|
||||||
|
[node name="right_line" type="MeshInstance3D" parent="RoadSurfaceMesh"]
|
||||||
|
unique_name_in_owner = true
|
||||||
|
transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.05, 0)
|
||||||
|
mesh = SubResource("ArrayMesh_n5r1a")
|
||||||
|
|
||||||
|
[node name="GlobalPathTraiectory" type="MeshInstance3D" parent="RoadSurfaceMesh"]
|
||||||
|
unique_name_in_owner = true
|
||||||
|
|
||||||
|
[node name="StaticBody3D" type="StaticBody3D" parent="RoadSurfaceMesh"]
|
||||||
|
|
||||||
|
[node name="CollisionShape3D" type="CollisionShape3D" parent="RoadSurfaceMesh/StaticBody3D"]
|
||||||
|
shape = SubResource("ConcavePolygonShape3D_t1jb5")
|
||||||
|
|
||||||
[connection signal="Onclick" from="MarginContainer/HBoxContainer/Title/Tabs" to="MarginContainer" method="_on_tabs_onclick"]
|
[connection signal="Onclick" from="MarginContainer/HBoxContainer/Title/Tabs" to="MarginContainer" method="_on_tabs_onclick"]
|
||||||
[connection signal="OnClick3DVehicle" from="MarginContainer/HBoxContainer/MarginContainer/DataPanel" to="MarginContainer" method="_on_data_panel_on_click_3d_vehicle"]
|
[connection signal="OnClick3DVehicle" from="MarginContainer/HBoxContainer/MarginContainer/DataPanel" to="MarginContainer" method="_on_data_panel_on_click_3d_vehicle"]
|
||||||
|
@ -8,7 +8,7 @@ Anim={
|
|||||||
"grid_snap_active": false,
|
"grid_snap_active": false,
|
||||||
"grid_step": Vector2(8, 8),
|
"grid_step": Vector2(8, 8),
|
||||||
"grid_visibility": 1,
|
"grid_visibility": 1,
|
||||||
"ofs": Vector2(196.355, -88.5674),
|
"ofs": Vector2(108.081, -220.644),
|
||||||
"primary_grid_step": Vector2i(8, 8),
|
"primary_grid_step": Vector2i(8, 8),
|
||||||
"show_group_gizmos": true,
|
"show_group_gizmos": true,
|
||||||
"show_guides": true,
|
"show_guides": true,
|
||||||
@ -34,7 +34,7 @@ Anim={
|
|||||||
"snap_rotation_step": 0.261799,
|
"snap_rotation_step": 0.261799,
|
||||||
"snap_scale": false,
|
"snap_scale": false,
|
||||||
"snap_scale_step": 0.1,
|
"snap_scale_step": 0.1,
|
||||||
"zoom": 0.749154
|
"zoom": 0.561231
|
||||||
}
|
}
|
||||||
3D={
|
3D={
|
||||||
"fov": 70.01,
|
"fov": 70.01,
|
||||||
@ -100,7 +100,7 @@ Anim={
|
|||||||
"auto_orthogonal_enabled": true,
|
"auto_orthogonal_enabled": true,
|
||||||
"cinematic_preview": false,
|
"cinematic_preview": false,
|
||||||
"display_mode": 21,
|
"display_mode": 21,
|
||||||
"distance": 4.0,
|
"distance": 6.29534,
|
||||||
"doppler": false,
|
"doppler": false,
|
||||||
"frame_time": false,
|
"frame_time": false,
|
||||||
"gizmos": true,
|
"gizmos": true,
|
||||||
@ -175,4 +175,4 @@ Anim={
|
|||||||
"zfar": 4000.01,
|
"zfar": 4000.01,
|
||||||
"znear": 0.05
|
"znear": 0.05
|
||||||
}
|
}
|
||||||
selected_nodes=Array[NodePath]([NodePath("/root/@EditorNode@16894/@Panel@13/@VBoxContainer@14/DockHSplitLeftL/DockHSplitLeftR/DockHSplitMain/@VBoxContainer@25/DockVSplitCenter/@VSplitContainer@52/@VBoxContainer@53/@PanelContainer@98/MainScreen/@CanvasItemEditor@9280/@VSplitContainer@9102/@HSplitContainer@9104/@HSplitContainer@9106/@Control@9107/@SubViewportContainer@9108/@SubViewport@9109/ItemLabel")])
|
selected_nodes=Array[NodePath]([])
|
||||||
|
@ -1,5 +1,5 @@
|
|||||||
[folding]
|
[folding]
|
||||||
|
|
||||||
node_unfolds=[NodePath("."), PackedStringArray("Layout", "Localization", "Tooltip", "Layout/Transform"), NodePath("TextureRect2"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/PixLogo"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button2"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button3"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/ShutdownIsScale"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/Title/Tabs"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/DataPanel"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Main3DVehicle"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Information"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Setting"), PackedStringArray("Visibility", "Layout", "Layout/Transform", "Theme Overrides", "Theme Overrides/constants"), NodePath("TextureRect"), PackedStringArray("Visibility", "Layout", "Mouse"), NodePath("Control"), PackedStringArray("Visibility", "Layout", "Mouse")]
|
node_unfolds=[NodePath("."), PackedStringArray("Layout", "Localization", "Tooltip", "Layout/Transform"), NodePath("TextureRect2"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/PixLogo"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer"), PackedStringArray("Layout", "Theme Overrides"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button2"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/VBoxContainer/Button3"), PackedStringArray("Layout", "Theme Overrides", "BiDi"), NodePath("MarginContainer/HBoxContainer/Title/VBoxContainer/ShutdownIsScale"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/Title/Tabs"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/DataPanel"), PackedStringArray("Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Main3DVehicle"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Information"), PackedStringArray("Visibility", "Layout"), NodePath("MarginContainer/HBoxContainer/MarginContainer/Setting"), PackedStringArray("Visibility", "Layout", "Layout/Transform", "Theme Overrides", "Theme Overrides/constants"), NodePath("TextureRect"), PackedStringArray("Visibility", "Layout", "Mouse"), NodePath("Control"), PackedStringArray("Visibility", "Layout", "Mouse")]
|
||||||
resource_unfolds=["res://control.tscn::StyleBoxFlat_kh08u", PackedStringArray("Resource"), "res://control.tscn::StyleBoxTexture_mkgnk", PackedStringArray("Resource"), "res://control.tscn::StyleBoxFlat_46du7", PackedStringArray("Resource", "Content Margins", "Corner Radius"), "res://control.tscn::StyleBoxFlat_78bhk", PackedStringArray("Resource", "Content Margins", "Corner Radius")]
|
resource_unfolds=["res://control.tscn::StyleBoxFlat_kh08u", PackedStringArray("Resource"), "res://control.tscn::StyleBoxTexture_mkgnk", PackedStringArray("Resource"), "res://control.tscn::StyleBoxFlat_46du7", PackedStringArray("Resource", "Content Margins", "Corner Radius"), "res://control.tscn::StyleBoxFlat_78bhk", PackedStringArray("Resource", "Content Margins", "Corner Radius"), "res://control.tscn::ArrayMesh_wx0gy", PackedStringArray(), "res://control.tscn::ArrayMesh_n5r1a", PackedStringArray(), "res://control.tscn::ConcavePolygonShape3D_t1jb5", PackedStringArray()]
|
||||||
nodes_folded=[]
|
nodes_folded=[NodePath("MarginContainer"), NodePath("RoadSurfaceMesh")]
|
||||||
|
@ -17,19 +17,19 @@ dock_filesystem_v_split_offset=0
|
|||||||
dock_filesystem_display_mode=0
|
dock_filesystem_display_mode=0
|
||||||
dock_filesystem_file_sort=0
|
dock_filesystem_file_sort=0
|
||||||
dock_filesystem_file_list_display_mode=1
|
dock_filesystem_file_list_display_mode=1
|
||||||
dock_filesystem_selected_paths=PackedStringArray("res://")
|
dock_filesystem_selected_paths=PackedStringArray("res://main.tscn")
|
||||||
dock_filesystem_uncollapsed_paths=PackedStringArray("Favorites", "res://", "res://Vehicle/", "res://pad_hmi_ui/taskbar/", "res://pad_hmi_ui/setting/", "res://pad_hmi_ui/information/", "res://pad_hmi_ui/home/", "res://Modules/")
|
dock_filesystem_uncollapsed_paths=PackedStringArray("Favorites", "res://", "res://pad_hmi_ui/taskbar/", "res://pad_hmi_ui/setting/", "res://pad_hmi_ui/information/", "res://pad_hmi_ui/home/")
|
||||||
dock_3="Scene,Import"
|
dock_3="Scene,Import"
|
||||||
dock_4="FileSystem"
|
dock_4="FileSystem"
|
||||||
dock_5="Inspector,Node,History"
|
dock_5="Inspector,Node,History"
|
||||||
|
|
||||||
[EditorNode]
|
[EditorNode]
|
||||||
|
|
||||||
open_scenes=PackedStringArray("res://control.tscn", "res://Modules/main_3d_vehicle.tscn", "res://Modules/setting.tscn", "res://Modules/margin_container_2.tscn", "res://Modules/data_panel.tscn", "res://Modules/item_label.tscn", "res://Modules/prompt_text.tscn")
|
open_scenes=PackedStringArray("res://control.tscn", "res://Modules/main_3d_vehicle.tscn", "res://Modules/setting.tscn", "res://Modules/margin_container_2.tscn", "res://Modules/data_panel.tscn", "res://Modules/item_label.tscn", "res://Modules/prompt_text.tscn", "res://main.tscn")
|
||||||
current_scene="res://Modules/data_panel.tscn"
|
current_scene="res://control.tscn"
|
||||||
center_split_offset=-491
|
center_split_offset=-491
|
||||||
selected_default_debugger_tab_idx=0
|
selected_default_debugger_tab_idx=0
|
||||||
selected_main_editor_idx=0
|
selected_main_editor_idx=1
|
||||||
selected_bottom_panel_item=0
|
selected_bottom_panel_item=0
|
||||||
|
|
||||||
[ScriptEditor]
|
[ScriptEditor]
|
||||||
|
File diff suppressed because one or more lines are too long
@ -5,7 +5,7 @@ use_advanced_connections=false
|
|||||||
|
|
||||||
[recent_files]
|
[recent_files]
|
||||||
|
|
||||||
scenes=["res://Modules/prompt_text.tscn", "res://Modules/item_label.tscn", "res://Modules/data_panel.tscn", "res://Modules/margin_container_2.tscn", "res://Modules/setting.tscn", "res://Modules/main_3d_vehicle.tscn", "res://control.tscn"]
|
scenes=["res://main.tscn", "res://Modules/prompt_text.tscn", "res://Modules/item_label.tscn", "res://Modules/data_panel.tscn", "res://Modules/margin_container_2.tscn", "res://Modules/setting.tscn", "res://Modules/main_3d_vehicle.tscn", "res://control.tscn"]
|
||||||
scripts=["Label", "InputEventMouseButton", "CanvasItem", "@GlobalScope", "res://code/hsliderPor.gd", "res://Modules/prompt_text.gd", "res://Modules/item_label.gd", "res://code/speedometer.gd", "res://Modules/SpeedGearAdjustment.gd", "res://Modules/site_selection_texture_rect.gd"]
|
scripts=["Label", "InputEventMouseButton", "CanvasItem", "@GlobalScope", "res://code/hsliderPor.gd", "res://Modules/prompt_text.gd", "res://Modules/item_label.gd", "res://code/speedometer.gd", "res://Modules/SpeedGearAdjustment.gd", "res://Modules/site_selection_texture_rect.gd"]
|
||||||
|
|
||||||
[dialog_bounds]
|
[dialog_bounds]
|
||||||
|
Binary file not shown.
374
app/src/main/assets/main.gd
Normal file
374
app/src/main/assets/main.gd
Normal file
@ -0,0 +1,374 @@
|
|||||||
|
extends Node3D
|
||||||
|
var rotation_d = [Vector2(-79.5,-90),Vector2(-20,-90)]
|
||||||
|
|
||||||
|
|
||||||
|
var camera_z = [20,10]
|
||||||
|
var camera_index = 0 :
|
||||||
|
set(value):
|
||||||
|
camera_index = value
|
||||||
|
var tween = get_tree().create_tween()
|
||||||
|
tween.set_parallel(true)
|
||||||
|
tween.tween_property(%Camera3D, "rotation_degrees",Vector3(0,rotation_d[camera_index].y,0), 1)
|
||||||
|
tween.tween_property(%Vertical, "rotation_degrees", Vector3(rotation_d[camera_index].x,0,0), 1)
|
||||||
|
tween.tween_property(%ViewCamera, "position", Vector3(0,2.687,camera_z[camera_index]), 1)
|
||||||
|
|
||||||
|
#func _init() -> void:
|
||||||
|
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
Global.scene3d = self
|
||||||
|
|
||||||
|
Global.sub_viewport = $SubViewport
|
||||||
|
Global.sub_viewport_2 = $SubViewport2
|
||||||
|
Websocket.connected_to_server.connect(Callable(self,"connected_to_server"))
|
||||||
|
Websocket.message_received.connect(Callable(self,"message_received"))
|
||||||
|
|
||||||
|
func connected_to_server():
|
||||||
|
update_turn_indicators_status()
|
||||||
|
update_actuation_status()
|
||||||
|
update_vehicle_door_report()
|
||||||
|
update_vcu_report1()
|
||||||
|
update_steering_status()
|
||||||
|
update_pose()
|
||||||
|
|
||||||
|
|
||||||
|
# teddy add
|
||||||
|
#update_vector_map_marker()
|
||||||
|
#update_trajectory()
|
||||||
|
#update_start_planner()
|
||||||
|
#update_pointcloud_map()
|
||||||
|
#update_predicted_objects()
|
||||||
|
|
||||||
|
# end
|
||||||
|
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 将四元数转换为欧拉角(roll, pitch, yaw)
|
||||||
|
func quaternion_to_euler(quaternion: Quaternion) -> Vector3:
|
||||||
|
var roll: float
|
||||||
|
var pitch: float
|
||||||
|
var yaw: float
|
||||||
|
|
||||||
|
# 计算四元数分量的平方
|
||||||
|
var sqx = quaternion.x * quaternion.x
|
||||||
|
var sqy = quaternion.y * quaternion.y
|
||||||
|
var sqz = quaternion.z * quaternion.z
|
||||||
|
var sqw = quaternion.w * quaternion.w
|
||||||
|
|
||||||
|
# 计算sarg,它代表pitch的sin值
|
||||||
|
var sarg = -2 * (quaternion.x * quaternion.z - quaternion.w * quaternion.y)
|
||||||
|
var pi_2 = PI/2 # π/2,表示90度的弧度值
|
||||||
|
|
||||||
|
# 处理万向节锁情况
|
||||||
|
if sarg <= -0.99999:
|
||||||
|
pitch = -pi_2 # pitch设为-90度
|
||||||
|
roll = 0 # roll设为0
|
||||||
|
yaw = 2 * atan2(quaternion.x, -quaternion.y) # 计算yaw角
|
||||||
|
elif sarg >= 0.99999:
|
||||||
|
pitch = pi_2 # pitch设为90度
|
||||||
|
roll = 0 # roll设为0
|
||||||
|
yaw = 2 * atan2(-quaternion.x, quaternion.y) # 计算yaw角
|
||||||
|
else:
|
||||||
|
# 在一般情况下,计算pitch, roll和yaw
|
||||||
|
pitch = asin(sarg)
|
||||||
|
roll = atan2(2 * (quaternion.y * quaternion.z + quaternion.w * quaternion.x), sqw - sqx - sqy + sqz)
|
||||||
|
yaw = atan2(2 * (quaternion.x * quaternion.y + quaternion.w * quaternion.z), sqw + sqx - sqy - sqz)
|
||||||
|
|
||||||
|
# 返回欧拉角,作为一个Vector3对象
|
||||||
|
return Vector3(roll, yaw, pitch)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
func message_received(data:Dictionary):
|
||||||
|
if data.is_empty():return
|
||||||
|
if !data.has("topic"):return
|
||||||
|
match data.topic:
|
||||||
|
"/localization/pose_twist_fusion_filter/pose":
|
||||||
|
var post = data.msg.pose
|
||||||
|
|
||||||
|
mb_post = Vector3(post.position.x, post.position.z, -post.position.y)
|
||||||
|
mb_rotation = quaternion_to_euler(Quaternion(post.orientation.x,post.orientation.y,post.orientation.z,post.orientation.w))
|
||||||
|
|
||||||
|
"/vehicle/status/steering_status":
|
||||||
|
var max_angle = 30
|
||||||
|
var min_angle = -30
|
||||||
|
var actual_angle = -data.msg.steering_tire_angle
|
||||||
|
#print(data.msg.steering_tire_angle)
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/wheel/Back_left
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/wheel/Back_left.rotation.y = actual_angle
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/wheel/Back_right.rotation.y = actual_angle
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/wheel/Front_left.rotation.y = -actual_angle
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/wheel/Front_right.rotation.y = -actual_angle
|
||||||
|
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/mudguard/mudguard_001.rotation.y = -actual_angle + 3.1316
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/mudguard/mudguard_002.rotation.y = -actual_angle + 3.1316
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/mudguard/mudguard_003.rotation.y = actual_angle + 3.1316
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/mudguard/mudguard_004.rotation.y = actual_angle + 3.1316
|
||||||
|
|
||||||
|
"/pix_robobus/vcu_report1":
|
||||||
|
match data.msg.turn_light_actual:
|
||||||
|
0:
|
||||||
|
%webcam.url = ""
|
||||||
|
#%webcam.request()
|
||||||
|
inite_zhuan()
|
||||||
|
1:
|
||||||
|
inite_zhuan()
|
||||||
|
|
||||||
|
%webcam.url = "http://192.168.4.88:8080/snapshot?topic=/camera1/image_raw"
|
||||||
|
%webcam.request()
|
||||||
|
|
||||||
|
#%LeftTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",true)
|
||||||
|
2:
|
||||||
|
inite_zhuan()
|
||||||
|
|
||||||
|
%webcam.url = "http://192.168.4.88:8080/snapshot?topic=/camera1/image_raw"
|
||||||
|
%webcam.request()
|
||||||
|
|
||||||
|
#%RightTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",true)
|
||||||
|
#3:
|
||||||
|
#%LeftTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",true)
|
||||||
|
#%RightTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",true)
|
||||||
|
|
||||||
|
_:
|
||||||
|
inite_zhuan()
|
||||||
|
|
||||||
|
"/vehicle/status/actuation_status":
|
||||||
|
if data.msg.status.brake_status <= 0.1:
|
||||||
|
#$EgoVehicle/VehicleBody3D/Light_Cover_2.set("surface_material_override/0",null)
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/lighting/brake_lights_2.set("surface_material_override/0",null)
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/lighting/brake_lights_4.set("surface_material_override/0",null)
|
||||||
|
|
||||||
|
else:
|
||||||
|
#$EgoVehicle/VehicleBody3D/Light_Cover_2.set("surface_material_override/0",load("res://sha_ce.material"))
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/lighting/brake_lights_2.set("surface_material_override/0",load("res://sha_ce.material"))
|
||||||
|
$EgoVehicle/robobus_a21_v11/robobus/lighting/brake_lights_4.set("surface_material_override/0",load("res://sha_ce.material"))
|
||||||
|
|
||||||
|
"/vehicle/status/velocity_status":
|
||||||
|
actuation_status = data.msg.longitudinal_velocity
|
||||||
|
#$MeshInstance3D.get("surface_material_override/0").set_shader_parameter("sd", data.msg.longitudinal_velocity *0.1)
|
||||||
|
|
||||||
|
"/pix_robobus/vehicle_door_report":
|
||||||
|
if(data.msg.door_open_inplace and (not data.msg.door_close_inplace)):
|
||||||
|
if current_animation != "开门":
|
||||||
|
current_animation = "开门"
|
||||||
|
#print( $AnimationPlayer.current_animation)
|
||||||
|
$AnimationPlayer.play("开门")
|
||||||
|
elif((not data.msg.door_open_inplace) and data.msg.door_close_inplace):
|
||||||
|
if current_animation != "关门":
|
||||||
|
current_animation = "关门"
|
||||||
|
$AnimationPlayer.play("关门")
|
||||||
|
else :
|
||||||
|
$AnimationPlayer.play("关门")
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
"/map/vector_map_marker":
|
||||||
|
if data.msg and data.msg.markers:
|
||||||
|
for marker in data.msg.markers:
|
||||||
|
var position = marker.pose.position
|
||||||
|
var x = position.x
|
||||||
|
var y = position.y
|
||||||
|
var z = position.z
|
||||||
|
#print("Position of marker: x = %.2f, y = %.2f, z = %.2f" % [x, y, z])
|
||||||
|
|
||||||
|
|
||||||
|
var color = marker.color
|
||||||
|
var r = color.r
|
||||||
|
var g = color.g
|
||||||
|
var b = color.b
|
||||||
|
var a = color.a
|
||||||
|
#print("Color of marker: r = %.2f, g = %.2f, b = %.2f, a = %.2f" % [r, g, b, a])
|
||||||
|
|
||||||
|
|
||||||
|
"/planning/scenario_planning/trajectory":
|
||||||
|
#var eps = 0.1
|
||||||
|
#var is_dy = true
|
||||||
|
#for point in data.msg.points:
|
||||||
|
#if eps < point.longitudinal_velocity_mps:
|
||||||
|
#print(point)
|
||||||
|
#is_dy = false
|
||||||
|
#break
|
||||||
|
#print(is_dy)
|
||||||
|
#print(data.msg.points[0].longitudinal_velocity_mps)
|
||||||
|
%WallTriangle.visible = 0.26 > data.msg.points[0].longitudinal_velocity_mps
|
||||||
|
|
||||||
|
|
||||||
|
#var post = data.msg.points.back().pose.position
|
||||||
|
#%WallTriangle.global_position = Vector3(post.x,post.z,-post.y)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
var current_animation = ""
|
||||||
|
func update_turn_indicators_status():#灯光反馈
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/vehicle/status/turn_indicators_status",
|
||||||
|
"type": "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_actuation_status():#刹车
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/vehicle/status/actuation_status",
|
||||||
|
"type": "tier4_vehicle_msgs/msg/ActuationStatusStamped"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_vehicle_door_report():#车门
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/pix_robobus/vehicle_door_report",
|
||||||
|
"type": "pix_robobus_driver_msgs/msg/VehicleDoorReport"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_steering_status():#转向弧度
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/vehicle/status/steering_status",
|
||||||
|
"type": "autoware_auto_vehicle_msgs/msg/SteeringReport"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_vcu_report1():#底盘反馈
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/pix_robobus/vcu_report1",
|
||||||
|
"type": "pix_robobus_driver_msgs/msg/VcuReport1"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_velocity_status():#车速
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/vehicle/status/velocity_status",
|
||||||
|
"type": "autoware_auto_vehicle_msgs/msg/VelocityReport"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# teddy add
|
||||||
|
func update_vector_map_marker(): # 可视化矢量地图
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/map/vector_map_marker",
|
||||||
|
"type": "visualization_msgs/msg/MarkerArray"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
func update_trajectory(): # 局部路径轨迹
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/scenario_planning/trajectory",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Trajectory"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
func update_start_planner(): # 全局路径轨迹
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/path_candidate/start_planner",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Path"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
func update_pointcloud_map(): # 点云地图
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/map/pointcloud_map",
|
||||||
|
"type": "sensor_msgs/msg/PointCloud2"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
func update_predicted_objects(): # 障碍物
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/perception/object_recognition/objects",
|
||||||
|
"type": "autoware_auto_perception_msgs/msg/PredictedObjects"
|
||||||
|
}
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
func update_pose(): # POSE
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/localization/pose_twist_fusion_filter/pose",
|
||||||
|
"type": "geometry_msgs/msg/PoseStamped"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(str(send_data))
|
||||||
|
|
||||||
|
func update_wall_triangle_status(): # wall
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/scenario_planning/trajectory",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Trajectory"
|
||||||
|
}
|
||||||
|
|
||||||
|
# end
|
||||||
|
|
||||||
|
|
||||||
|
func inite_zhuan():#初始化转向灯
|
||||||
|
return
|
||||||
|
%LeftTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",false)
|
||||||
|
%RightTurnLights.get("surface_material_override/0").set_shader_parameter("is_shandeng",false)
|
||||||
|
|
||||||
|
var actuation_status = 0
|
||||||
|
var mb_post = Vector3(0,0,0)
|
||||||
|
var mb_rotation = Vector3(0,0,0)
|
||||||
|
func _process(delta):
|
||||||
|
if mb_post.distance_to($EgoVehicle.global_position) > 10:
|
||||||
|
$EgoVehicle.global_position = mb_post
|
||||||
|
$EgoVehicle.rotation = mb_rotation
|
||||||
|
else:
|
||||||
|
$EgoVehicle.global_position = lerp($EgoVehicle.global_position,mb_post,1*delta)
|
||||||
|
$EgoVehicle.rotation.x = lerp_angle($EgoVehicle.rotation.x,mb_rotation.x,1*delta)
|
||||||
|
$EgoVehicle.rotation.y = lerp_angle($EgoVehicle.rotation.y,mb_rotation.y,1*delta)
|
||||||
|
$EgoVehicle.rotation.z = lerp_angle($EgoVehicle.rotation.z,mb_rotation.z,1*delta)
|
||||||
|
|
||||||
|
xz(delta*500*actuation_status)
|
||||||
|
|
||||||
|
$SubViewport/Camera3D.global_position.x = $EgoVehicle.global_position.x
|
||||||
|
$SubViewport/Camera3D.global_position.z = $EgoVehicle.global_position.z
|
||||||
|
$SubViewport/Camera3D.global_position.y = $EgoVehicle.global_position.y + 70
|
||||||
|
$SubViewport/Camera3D.global_rotation_degrees.y = $EgoVehicle.global_rotation_degrees.y - 90
|
||||||
|
$SubViewport2/Camera3D.global_transform = %ViewCamera.global_transform
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
var jd = 360
|
||||||
|
func xz(delta):
|
||||||
|
jd -= delta
|
||||||
|
if jd <= 0:
|
||||||
|
jd = 360
|
||||||
|
var arr = [$EgoVehicle/robobus_a21_v11/robobus/wheel/Back_left, $EgoVehicle/robobus_a21_v11/robobus/wheel/Back_right, $EgoVehicle/robobus_a21_v11/robobus/wheel/Front_left, $EgoVehicle/robobus_a21_v11/robobus/wheel/Front_right]
|
||||||
|
|
||||||
|
for item in arr:
|
||||||
|
item.rotation_degrees.z = jd * -1
|
||||||
|
|
||||||
|
|
||||||
|
var is_move = true
|
||||||
|
func _input(event: InputEvent) -> void:
|
||||||
|
if event is InputEventScreenPinch:#手指捏合
|
||||||
|
%ViewCamera.position.z -= event.relative * 0.01
|
||||||
|
is_move = false
|
||||||
|
$EgoVehicle/Timer.start()
|
||||||
|
|
||||||
|
func _on_control_gui_input(event):
|
||||||
|
if event is InputEventScreenDrag and is_move:
|
||||||
|
%Vertical.rotation.y -= event.relative.x * 0.01
|
||||||
|
%Vertical.rotation.x -= event.relative.y * 0.01
|
||||||
|
%Vertical.rotation_degrees.x = clampf(%Vertical.rotation_degrees.x,-90,0 )
|
||||||
|
#if %Vertical.rotation.x >0 :
|
||||||
|
#%Vertical.rotation.x = 0
|
||||||
|
#if %Vertical.rotation.x > -90 :
|
||||||
|
#%Vertical.rotation.x = -90
|
||||||
|
pass # Replace with function body.
|
||||||
|
|
||||||
|
|
||||||
|
func _on_timer_timeout() -> void:
|
||||||
|
is_move = true
|
||||||
|
pass # Replace with function body.
|
File diff suppressed because one or more lines are too long
43
app/src/main/assets/scripts/csg_mesh_3d.gd
Normal file
43
app/src/main/assets/scripts/csg_mesh_3d.gd
Normal file
@ -0,0 +1,43 @@
|
|||||||
|
@tool
|
||||||
|
extends Node3D
|
||||||
|
|
||||||
|
@export var a = false:
|
||||||
|
set(value):
|
||||||
|
var arr = [Vector3(428.2168, 8.623453, -227.8385), Vector3(428.1051, 8.6564, -229.2604), Vector3(426.622, 8.9364, -242.6571), Vector3(426.0689, 9.0102, -246.3827), Vector3(426.0144, 9.0102, -246.7882), Vector3(425.964, 9.0102, -247.249), Vector3(425.9302, 9.0102, -247.684), Vector3(425.9099, 9.0102, -248.1199), Vector3(425.9033, 9.0102, -248.5564), Vector3(425.9103, 9.0102, -248.993), Vector3(425.9309, 9.0102, -249.4293), Vector3(425.9652, 9.0102, -249.8648), Vector3(426.0131, 9.0102, -250.2991), Vector3(426.0745, 9.0102, -250.7319), Vector3(426.1495, 9.0102, -251.1626), Vector3(426.2379, 9.0102, -251.5909), Vector3(426.3396, 9.0102, -252.0163), Vector3(426.4546, 9.0102, -252.4385), Vector3(426.5828, 9.0102, -252.857), Vector3(426.7241, 9.0102, -253.2713), Vector3(426.8783, 9.0102, -253.6812), Vector3(427.0452, 9.0102, -254.0861), Vector3(427.2247, 9.0102, -254.4857), Vector3(427.4168, 9.0102, -254.8796), Vector3(427.621, 9.0102, -255.2674), Vector3(427.8374, 9.0102, -255.6488), Vector3(428.0656, 9.0102, -256.0233), Vector3(428.3054, 9.0102, -256.3906), Vector3(428.5567, 9.0102, -256.7503), Vector3(428.8192, 9.0102, -257.102), Vector3(429.0926, 9.0102, -257.4455), Vector3(429.3766, 9.0102, -257.7804), Vector3(429.6711, 9.0102, -258.1063), Vector3(429.9757, 9.0102, -258.4229), Vector3(430.2901, 9.0102, -258.7299), Vector3(430.614, 9.0102, -259.027), Vector3(430.9472, 9.0102, -259.314), Vector3(431.2892, 9.0102, -259.5904), Vector3(431.6398, 9.0102, -259.8561), Vector3(431.9986, 9.0102, -260.1108), Vector3(432.3653, 9.0102, -260.3542), Vector3(432.7395, 9.0102, -260.5861), Vector3(433.1209, 9.0102, -260.8063), Vector3(433.509, 9.0102, -261.0145), Vector3(433.9036, 9.0102, -261.2105), Vector3(434.3042, 9.0102, -261.3941), Vector3(434.7104, 9.0102, -261.5651), Vector3(435.122, 9.0102, -261.7235), Vector3(435.5383, 9.0102, -261.8689), Vector3(435.9591, 9.0102, -262.0013), Vector3(436.384, 9.0102, -262.1205), Vector3(436.8124, 9.0102, -262.2263), Vector3(437.2441, 9.0102, -262.3188), Vector3(437.6786, 9.0102, -262.3978), Vector3(438.1397, 9.0102, -262.4664), Vector3(441.5602, 9.6755, -262.7914), Vector3(471.8963, 10.5332, -265.4572), Vector3(508.307, 11.5039, -268.558), Vector3(550.8515, 12.7924, -272.3389), Vector3(570.2333, 13.32565, -274.1788)]
|
||||||
|
add_ptah(arr,$CSGMesh3D)
|
||||||
|
func add_ptah(arr,node):
|
||||||
|
|
||||||
|
var vertices = PackedVector3Array()
|
||||||
|
for index in arr.size():
|
||||||
|
if index >= arr.size() -1: break
|
||||||
|
var a_post:Vector3 = arr[index]
|
||||||
|
var b_post:Vector3 = arr[index + 1]
|
||||||
|
var a1 = calculate_A2_position(Vector2(a_post.x,a_post.z),Vector2(b_post.x,b_post.z),1)
|
||||||
|
vertices.push_back(a_post)
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
var b1 = calculate_A2_position(Vector2(b_post.x,b_post.z),Vector2(a_post.x,a_post.z),1)
|
||||||
|
vertices.push_back(Vector3(b1.x,b_post.y,b1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
# 初始化 ArrayMesh。
|
||||||
|
var arr_mesh = ArrayMesh.new()
|
||||||
|
var arrays = []
|
||||||
|
arrays.resize(Mesh.ARRAY_MAX)
|
||||||
|
arrays[Mesh.ARRAY_VERTEX] = vertices
|
||||||
|
# 创建 Mesh。
|
||||||
|
arr_mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arrays)
|
||||||
|
node.mesh = arr_mesh
|
||||||
|
pass
|
||||||
|
func calculate_A2_position(A1: Vector2, B1: Vector2, distance_to_A2: float) -> Vector2:
|
||||||
|
var m_l2 = (B1.y - A1.y) / (B1.x - A1.x) # 计算m_l2 (B1 和 A1之间的斜率)
|
||||||
|
var m_l1 = -1 / m_l2 # 计算m_l1 (垂直线的斜率)
|
||||||
|
var dx = distance_to_A2 / sqrt(1 + pow(m_l1, 2)) # 计算 dx 和 dy
|
||||||
|
var dy = m_l1 * dx
|
||||||
|
# 计算 A2 的位置 (有两个可能的解)
|
||||||
|
var A2_1 = Vector2(A1.x + dx, A1.y + dy)
|
||||||
|
var A2_2 = Vector2(A1.x - dx, A1.y - dy)
|
||||||
|
# 你可以根据实际情况返回其中一个解
|
||||||
|
return A2_1 # 此处默认返回第一个解 A2_1
|
84
app/src/main/assets/scripts/dynamic_object_mesh.gd
Normal file
84
app/src/main/assets/scripts/dynamic_object_mesh.gd
Normal file
@ -0,0 +1,84 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var dynamic_objects = DynamicObjects.new()
|
||||||
|
var only_known_object = true
|
||||||
|
var object_3d_model_mode = false
|
||||||
|
|
||||||
|
var pool_size = 100
|
||||||
|
var model_types = {
|
||||||
|
"car": {"scene": preload("res://DynamicObject/Car.tscn"), "pool": []},
|
||||||
|
"pedestrian": {"scene": preload("res://DynamicObject/Pedestrian.tscn"), "pool": []},
|
||||||
|
"truck": {"scene": preload("res://DynamicObject/Truck.tscn"), "pool": []},
|
||||||
|
"trailer": {"scene": preload("res://DynamicObject/Trailer.tscn"), "pool": []},
|
||||||
|
"bus": {"scene": preload("res://DynamicObject/Bus.tscn"), "pool": []},
|
||||||
|
"bicycle": {"scene": preload("res://DynamicObject/Bicycle.tscn"), "pool": []},
|
||||||
|
"motorcycle": {"scene": preload("res://DynamicObject/Motorcycle.tscn"), "pool": []}
|
||||||
|
}
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
dynamic_objects.subscribe("/perception/object_recognition/objects", false)
|
||||||
|
initialize_model_pools()
|
||||||
|
|
||||||
|
func initialize_model_pools():
|
||||||
|
for model_type in model_types:
|
||||||
|
for i in range(pool_size):
|
||||||
|
var node = model_types[model_type]["scene"].instantiate()
|
||||||
|
node.visible = false
|
||||||
|
add_child(node)
|
||||||
|
model_types[model_type]["pool"].append(node)
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if not dynamic_objects.has_new():
|
||||||
|
return
|
||||||
|
|
||||||
|
if object_3d_model_mode:
|
||||||
|
var object_list = dynamic_objects.get_dynamic_object_list(only_known_object)
|
||||||
|
reset_visibility()
|
||||||
|
update_models(object_list)
|
||||||
|
else:
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
var normals = PackedVector3Array()
|
||||||
|
|
||||||
|
var triangle_list = dynamic_objects.get_triangle_list(only_known_object)
|
||||||
|
|
||||||
|
for point in triangle_list:
|
||||||
|
verts.append(point["position"])
|
||||||
|
normals.append(point["normal"])
|
||||||
|
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
arr[Mesh.ARRAY_NORMAL] = normals
|
||||||
|
|
||||||
|
if !verts.is_empty():
|
||||||
|
reset_visibility()
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
|
||||||
|
dynamic_objects.set_old()
|
||||||
|
|
||||||
|
func reset_visibility():
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
for model_type in model_types:
|
||||||
|
for node in model_types[model_type]["pool"]:
|
||||||
|
node.visible = false
|
||||||
|
|
||||||
|
func update_models(object_list):
|
||||||
|
var model_indices = {}
|
||||||
|
for model_type in model_types.keys():
|
||||||
|
model_indices[model_type] = 0
|
||||||
|
for object in object_list:
|
||||||
|
var model_type = object["class"]
|
||||||
|
if model_type in model_types:
|
||||||
|
var pool = model_types[model_type]["pool"]
|
||||||
|
var index = model_indices[model_type]
|
||||||
|
if index < pool.size():
|
||||||
|
var node = pool[index]
|
||||||
|
node.visible = true
|
||||||
|
node.set_position(Vector3(object["position"].x, object["position"].y - object["size"].y * 0.5, object["position"].z))
|
||||||
|
node.set_rotation(object["rotation"])
|
||||||
|
model_indices[model_type] += 1
|
||||||
|
|
||||||
|
func _on_OnlyKnownObjectCheckButton_toggled(button_pressed):
|
||||||
|
only_known_object = button_pressed
|
||||||
|
|
||||||
|
func _on_d_model_object_toggled(toggled_on):
|
||||||
|
object_3d_model_mode = toggled_on
|
47
app/src/main/assets/scripts/obstacle_segmentation_mesh.gd
Normal file
47
app/src/main/assets/scripts/obstacle_segmentation_mesh.gd
Normal file
@ -0,0 +1,47 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var pointcloud = PointCloud.new()
|
||||||
|
var sum_time = 0.0
|
||||||
|
var transparency_speed = 4.0
|
||||||
|
var transparency_scale = 0.3 # 0.0~0.5
|
||||||
|
var cycle_time = 3.0
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
pointcloud.subscribe("/perception/obstacle_segmentation/pointcloud", false)
|
||||||
|
|
||||||
|
func _process(delta):
|
||||||
|
sum_time += delta
|
||||||
|
|
||||||
|
if sum_time * transparency_speed < 2 * PI:
|
||||||
|
transparency = clamp(cos(sum_time * transparency_speed) * transparency_scale + (1 - transparency_scale), 0.0, 1.0)
|
||||||
|
else:
|
||||||
|
transparency = 1.0
|
||||||
|
if (cycle_time < sum_time):
|
||||||
|
sum_time = 0.0
|
||||||
|
|
||||||
|
|
||||||
|
if !pointcloud.has_new():
|
||||||
|
return
|
||||||
|
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
# var uvs = PoolVector2Array()
|
||||||
|
# var normals = PoolVector3Array()
|
||||||
|
# var indices = PoolIntArray()
|
||||||
|
|
||||||
|
verts = pointcloud.get_pointcloud("map")
|
||||||
|
# for vert in verts:
|
||||||
|
# uvs.append(Vector2(vert.y, 0))
|
||||||
|
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
# arr[Mesh.ARRAY_TEX_UV] = uvs
|
||||||
|
|
||||||
|
if !verts.is_empty():
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_POINTS, arr)
|
||||||
|
pointcloud.set_old()
|
||||||
|
|
||||||
|
|
||||||
|
func _on_enable_obstacle_segmentation_button_toggled(toggled_on):
|
||||||
|
visible = toggled_on
|
50
app/src/main/assets/scripts/path_drivable_area_mesh.gd
Normal file
50
app/src/main/assets/scripts/path_drivable_area_mesh.gd
Normal file
@ -0,0 +1,50 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var path = BehaviorPath.new()
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
path.subscribe("/planning/scenario_planning/lane_driving/behavior_planning/path", false)
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if !path.has_new():
|
||||||
|
return
|
||||||
|
|
||||||
|
# Drivable area
|
||||||
|
var drivable_area_triangle_strip = path.get_drivable_area_triangle_strip(0.1)
|
||||||
|
|
||||||
|
# Left line
|
||||||
|
var left_line_triangle_strip = drivable_area_triangle_strip["left_line"]
|
||||||
|
var left_line_arr = []
|
||||||
|
left_line_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var left_line_verts = PackedVector3Array()
|
||||||
|
var left_line_normals = PackedVector3Array()
|
||||||
|
var left_line_colors = PackedColorArray()
|
||||||
|
for point in left_line_triangle_strip:
|
||||||
|
left_line_verts.append(point["position"])
|
||||||
|
left_line_normals.append(point["normal"])
|
||||||
|
left_line_colors.append(Color(0.0, 0.25, 1.0, 0.9))
|
||||||
|
left_line_arr[Mesh.ARRAY_VERTEX] = left_line_verts
|
||||||
|
left_line_arr[Mesh.ARRAY_NORMAL] = left_line_normals
|
||||||
|
left_line_arr[Mesh.ARRAY_COLOR] = left_line_colors
|
||||||
|
|
||||||
|
# Right line
|
||||||
|
var right_line_triangle_strip = drivable_area_triangle_strip["right_line"]
|
||||||
|
var right_line_arr = []
|
||||||
|
right_line_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var right_line_verts = PackedVector3Array()
|
||||||
|
var right_line_normals = PackedVector3Array()
|
||||||
|
var right_line_colors = PackedColorArray()
|
||||||
|
for point in right_line_triangle_strip:
|
||||||
|
right_line_verts.append(point["position"])
|
||||||
|
right_line_normals.append(point["normal"])
|
||||||
|
right_line_colors.append(Color(0.0, 0.25, 1.0, 0.9))
|
||||||
|
right_line_arr[Mesh.ARRAY_VERTEX] = right_line_verts
|
||||||
|
right_line_arr[Mesh.ARRAY_NORMAL] = right_line_normals
|
||||||
|
right_line_arr[Mesh.ARRAY_COLOR] = right_line_colors
|
||||||
|
|
||||||
|
if !left_line_verts.is_empty():
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, left_line_arr)
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, right_line_arr)
|
||||||
|
path.set_old()
|
||||||
|
|
34
app/src/main/assets/scripts/point_cloud_map_mesh.gd
Normal file
34
app/src/main/assets/scripts/point_cloud_map_mesh.gd
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var pointcloud = PointCloud.new()
|
||||||
|
var visualize_again = false
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
pointcloud.subscribe("/map/pointcloud_map", true)
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if not (pointcloud.has_new() or visualize_again):
|
||||||
|
return
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
# var uvs = PoolVector2Array()
|
||||||
|
# var normals = PoolVector3Array()
|
||||||
|
# var indices = PoolIntArray()
|
||||||
|
|
||||||
|
verts = pointcloud.get_pointcloud("map")
|
||||||
|
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
# arr[Mesh.ARRAY_TEX_UV] = uvs
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_POINTS, arr)
|
||||||
|
visualize_again = false
|
||||||
|
pointcloud.set_old()
|
||||||
|
|
||||||
|
func _on_CheckButton_toggled(button_pressed):
|
||||||
|
|
||||||
|
visible = button_pressed
|
||||||
|
if not visible:
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
elif visible:
|
||||||
|
visualize_again = true
|
19
app/src/main/assets/scripts/road_marker_mesh.gd
Normal file
19
app/src/main/assets/scripts/road_marker_mesh.gd
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
|
||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
func visualize_mesh(triangle_list):
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
var normals = PackedVector3Array()
|
||||||
|
# var colors = PoolColorArray()
|
||||||
|
|
||||||
|
for point in triangle_list:
|
||||||
|
verts.append(point["position"])
|
||||||
|
normals.append(Vector3(0,1,0))
|
||||||
|
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
arr[Mesh.ARRAY_NORMAL] = normals
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
|
376
app/src/main/assets/scripts/road_surface_mesh.gd
Normal file
376
app/src/main/assets/scripts/road_surface_mesh.gd
Normal file
@ -0,0 +1,376 @@
|
|||||||
|
#@tool
|
||||||
|
extends Node3D
|
||||||
|
|
||||||
|
# 模拟对象分类标签的常量
|
||||||
|
enum label {
|
||||||
|
UNKNOWN,
|
||||||
|
CAR,
|
||||||
|
TRUCK,
|
||||||
|
BUS,
|
||||||
|
TRAILER,
|
||||||
|
MOTORCYCLE,
|
||||||
|
BICYCLE,
|
||||||
|
PEDESTRIAN
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
# 定义不同类型对象的模型池,包含预加载的场景资源和对象池
|
||||||
|
var model_types = {
|
||||||
|
"car": {"scene": preload("res://DynamicObject/Car.tscn"), "pool": []},
|
||||||
|
"pedestrian": {"scene": preload("res://DynamicObject/Pedestrian.tscn"), "pool": []},
|
||||||
|
"truck": {"scene": preload("res://DynamicObject/Truck.tscn"), "pool": []},
|
||||||
|
"trailer": {"scene": preload("res://DynamicObject/Trailer.tscn"), "pool": []},
|
||||||
|
"bus": {"scene": preload("res://DynamicObject/Bus.tscn"), "pool": []},
|
||||||
|
"bicycle": {"scene": preload("res://DynamicObject/Bicycle.tscn"), "pool": []},
|
||||||
|
"motorcycle": {"scene": preload("res://DynamicObject/Motorcycle.tscn"), "pool": []},
|
||||||
|
"renwu": {"scene": preload("res://DynamicObject/renwu.tscn"), "pool": []}
|
||||||
|
}
|
||||||
|
func _ready() -> void:
|
||||||
|
|
||||||
|
Websocket.connected_to_server.connect(Callable(self, "connected_to_server"))
|
||||||
|
Websocket.message_received.connect(Callable(self, "message_received"))
|
||||||
|
|
||||||
|
|
||||||
|
func connected_to_server():
|
||||||
|
update_vector_map_marker()
|
||||||
|
update_dynamic_object_marker()
|
||||||
|
update_path_drivable_area_marker()
|
||||||
|
update_trajectory_marker()
|
||||||
|
update_global_path_traiectory()
|
||||||
|
|
||||||
|
%LoadingPage.set_jd(50)
|
||||||
|
pass # Replace with function body
|
||||||
|
|
||||||
|
func message_received(data: Dictionary):
|
||||||
|
if data.is_empty():
|
||||||
|
return
|
||||||
|
if !data.has("topic"):return
|
||||||
|
match data.topic:
|
||||||
|
"/map/vector_map_marker":
|
||||||
|
var vector_map_list = []
|
||||||
|
var vector_map_list_ns = []
|
||||||
|
for pos in data.msg.markers:
|
||||||
|
for marker in pos.points: # visualisation all
|
||||||
|
vector_map_list.append(Vector3(marker.x, marker.z, -marker.y))
|
||||||
|
|
||||||
|
visualize_mesh(vector_map_list)
|
||||||
|
|
||||||
|
# visualisation parking space chaging color
|
||||||
|
if pos.ns == "parking_space":
|
||||||
|
#print("Detected parking space marker with id: ", pos.id)
|
||||||
|
var a = func ():
|
||||||
|
# parking space 的坐标
|
||||||
|
for node in %ParkingSpace.get_children():
|
||||||
|
node.queue_free()
|
||||||
|
var verts = []
|
||||||
|
for i in pos.points.size()/6:
|
||||||
|
vector_map_list_ns = []
|
||||||
|
|
||||||
|
for index in 6:
|
||||||
|
var _marker = pos.points[(i*6)+index]
|
||||||
|
#print((i*6)+index)
|
||||||
|
vector_map_list_ns.append(Vector3(_marker.x, _marker.z, -_marker.y))
|
||||||
|
|
||||||
|
verts.append(Vector3(_marker.x, _marker.z, -_marker.y))
|
||||||
|
|
||||||
|
visualize_mesh_ps(vector_map_list_ns)
|
||||||
|
#await get_tree().create_timer(2).timeout
|
||||||
|
#$StaticBody3D/CollisionShape3D.shape = ConcavePolygonShape3D.new()
|
||||||
|
#$StaticBody3D/CollisionShape3D.shape.set_faces(verts)
|
||||||
|
a.call()
|
||||||
|
|
||||||
|
%LoadingPage.set_jd(100)
|
||||||
|
|
||||||
|
|
||||||
|
"/perception/object_recognition/objects":
|
||||||
|
var objects_list = {}
|
||||||
|
for obj in data.msg.objects:
|
||||||
|
if obj.classification[0].label == label.UNKNOWN:
|
||||||
|
continue
|
||||||
|
|
||||||
|
var pos = obj.kinematics.initial_pose_with_covariance.pose.position
|
||||||
|
var quat = obj.kinematics.initial_pose_with_covariance.pose.orientation
|
||||||
|
var shape = obj.shape
|
||||||
|
|
||||||
|
# 将字典转换为 Quaternion 对象
|
||||||
|
var my_quat = dictionary_to_quaternion(quat)
|
||||||
|
|
||||||
|
var euler_angles = quat_to_euler(my_quat)
|
||||||
|
var roll = euler_angles.x
|
||||||
|
var pitch = euler_angles.y
|
||||||
|
var yaw = euler_angles.z
|
||||||
|
|
||||||
|
#print("test yaw data: ", yaw)
|
||||||
|
|
||||||
|
var dynamic_object = {
|
||||||
|
"position": ros2_to_godot(pos.x, pos.y, pos.z),
|
||||||
|
|
||||||
|
"rotation": ros2_to_godot(roll, pitch, yaw),
|
||||||
|
"size": ros2_to_godot(shape.dimensions.x, shape.dimensions.y, shape.dimensions.z)
|
||||||
|
}
|
||||||
|
|
||||||
|
var calss_name = ret_class(obj.classification[0].label)
|
||||||
|
if !objects_list.has(calss_name):
|
||||||
|
objects_list[calss_name] = []
|
||||||
|
objects_list[calss_name].append(dynamic_object)
|
||||||
|
|
||||||
|
visualize_dynamic_objects(objects_list)
|
||||||
|
|
||||||
|
# path drivable area marker
|
||||||
|
"/planning/scenario_planning/lane_driving/behavior_planning/path":
|
||||||
|
var arr = []
|
||||||
|
var arr2 = []
|
||||||
|
for post in data.msg.left_bound:
|
||||||
|
arr.append(Vector3(post.x,post.z,-post.y))
|
||||||
|
for post in data.msg.right_bound:
|
||||||
|
arr2.append(Vector3(post.x,post.z,-post.y))
|
||||||
|
add_ptah(arr2,%left_line)
|
||||||
|
add_ptah(arr,%right_line)
|
||||||
|
|
||||||
|
|
||||||
|
var vertices = []
|
||||||
|
for point in data.msg.points:
|
||||||
|
var post = (point.pose.position)
|
||||||
|
vertices.push_back(Vector3(post.x, post.z, -post.y))
|
||||||
|
pass
|
||||||
|
#%GlobalPathTraiectory.add_ptah(vertices,%GlobalPathTraiectory,preload("res://material/line_4.tres"))
|
||||||
|
|
||||||
|
"/planning/scenario_planning/trajectory":
|
||||||
|
var vertices = []
|
||||||
|
for point in data.msg.points:
|
||||||
|
var post = (point.pose.position)
|
||||||
|
vertices.push_back(Vector3(post.x, post.z, -post.y))
|
||||||
|
pass
|
||||||
|
$Traiectory.add_ptah(vertices,$Traiectory)
|
||||||
|
|
||||||
|
|
||||||
|
"/planning/path_candidate/start_planner":
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
func ret_class(id):
|
||||||
|
match id:
|
||||||
|
label.PEDESTRIAN:
|
||||||
|
#return"pedestrian"
|
||||||
|
return"renwu"
|
||||||
|
label.BICYCLE:
|
||||||
|
return"bicycle"
|
||||||
|
label.CAR:
|
||||||
|
return"car"
|
||||||
|
label.TRUCK:
|
||||||
|
return"truck"
|
||||||
|
label.MOTORCYCLE:
|
||||||
|
return"motorcycle"
|
||||||
|
label.BUS:
|
||||||
|
return"bus"
|
||||||
|
label.TRAILER:
|
||||||
|
return"trailer"
|
||||||
|
_:
|
||||||
|
return"unknown"
|
||||||
|
# 将字典转换为 Quaternion
|
||||||
|
func dictionary_to_quaternion(quat_dict: Dictionary) -> Quaternion:
|
||||||
|
return Quaternion(quat_dict["x"], quat_dict["y"], quat_dict["z"], quat_dict["w"])
|
||||||
|
|
||||||
|
|
||||||
|
# 将四元数转换为欧拉角的函数
|
||||||
|
func quat_to_euler(quat: Quaternion) -> Vector3:
|
||||||
|
var basis = Basis(quat) # 使用四元数创建一个 Basis 对象
|
||||||
|
var euler = basis.get_euler() # 获取欧拉角,返回一个包含滚转、俯仰和偏航角的 Vector3
|
||||||
|
return euler
|
||||||
|
|
||||||
|
# parking space 可视化函数
|
||||||
|
func visualize_mesh_ps(parking_space_list):
|
||||||
|
var st = StaticBody3D.new()
|
||||||
|
%ParkingSpace.add_child(st)
|
||||||
|
var co = CollisionShape3D.new()
|
||||||
|
st.add_child(co)
|
||||||
|
co.shape = ConcavePolygonShape3D.new()
|
||||||
|
co.shape.set_faces(parking_space_list)
|
||||||
|
|
||||||
|
var node = MeshInstance3D.new()
|
||||||
|
st.add_child(node)
|
||||||
|
node.name = "mesh"
|
||||||
|
|
||||||
|
|
||||||
|
node.mesh = ArrayMesh.new()
|
||||||
|
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
var normals = PackedVector3Array()
|
||||||
|
|
||||||
|
for point in parking_space_list:
|
||||||
|
verts.append(point)
|
||||||
|
normals.append(Vector3(0, 1, 0))
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
arr[Mesh.ARRAY_NORMAL] = normals
|
||||||
|
node.mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
|
||||||
|
|
||||||
|
|
||||||
|
node.mesh.surface_set_material(0,preload("res://ParkingSpace.tres")) # parking space color
|
||||||
|
|
||||||
|
# 可视化路面的函数
|
||||||
|
func visualize_mesh(triangle_list):
|
||||||
|
$RoadSurface.mesh = ArrayMesh.new()
|
||||||
|
|
||||||
|
var arr = []
|
||||||
|
arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var verts = PackedVector3Array()
|
||||||
|
var normals = PackedVector3Array()
|
||||||
|
|
||||||
|
for point in triangle_list:
|
||||||
|
verts.append(point)
|
||||||
|
normals.append(Vector3(0, 1, 0))
|
||||||
|
arr[Mesh.ARRAY_VERTEX] = verts
|
||||||
|
arr[Mesh.ARRAY_NORMAL] = normals
|
||||||
|
$RoadSurface.mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
|
||||||
|
|
||||||
|
$RoadSurface.mesh.surface_set_material(0,preload("res://line_3.tres"))
|
||||||
|
|
||||||
|
func visualize_dynamic_objects(objects_list):
|
||||||
|
for key in objects_list.keys():
|
||||||
|
|
||||||
|
var cha_zhi = model_types[key].pool.size() - objects_list[key].size()
|
||||||
|
if cha_zhi <= 0:
|
||||||
|
for i in abs(cha_zhi):
|
||||||
|
var node = model_types[key].scene.instantiate()
|
||||||
|
model_types[key].pool.append(node)
|
||||||
|
node.visible = false
|
||||||
|
$DynamicObject.add_child(node)
|
||||||
|
for node in model_types[key].pool:
|
||||||
|
node.visible = false
|
||||||
|
for index in objects_list[key].size():
|
||||||
|
var obj = objects_list[key][index]
|
||||||
|
var node = model_types[key].pool[index]
|
||||||
|
node.visible = true
|
||||||
|
#node.position = obj.position
|
||||||
|
#node.rotation = obj.rotation
|
||||||
|
#node.scale = obj.size
|
||||||
|
|
||||||
|
node.set_position(Vector3(obj["position"].x, obj["position"].y - obj["size"].y * 0.5, obj["position"].z))
|
||||||
|
node.set_rotation(obj["rotation"])
|
||||||
|
|
||||||
|
pass
|
||||||
|
|
||||||
|
func add_ptah(arr,node):
|
||||||
|
#print(arr)
|
||||||
|
var vertices = PackedVector3Array()
|
||||||
|
for index in arr.size():
|
||||||
|
if arr.size() <=2:
|
||||||
|
if index >= arr.size() -2: break
|
||||||
|
var a_post:Vector3 = arr[index]
|
||||||
|
var b_post:Vector3 = arr[index + 1]
|
||||||
|
var a1 = calculate_A2_position(Vector2(a_post.x,a_post.z),Vector2(b_post.x,b_post.z),0.1)
|
||||||
|
|
||||||
|
vertices.push_back(a_post)
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
var b1 = calculate_A2_position(Vector2(b_post.x,b_post.z),Vector2(a_post.x,a_post.z),0.1)
|
||||||
|
|
||||||
|
vertices.push_back(Vector3(b1.x,b_post.y,b1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
else:
|
||||||
|
if index >= arr.size() -2: break
|
||||||
|
var a_post:Vector3 = arr[index]
|
||||||
|
var b_post:Vector3 = arr[index + 1]
|
||||||
|
var c_post:Vector3 = arr[index + 2]
|
||||||
|
var a1 = calculate_A2_position(Vector2(a_post.x,a_post.z),Vector2(b_post.x,b_post.z),0.1)
|
||||||
|
|
||||||
|
vertices.push_back(a_post)
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
var b1 = calculate_A2_position(Vector2(b_post.x,b_post.z),Vector2(c_post.x,c_post.z),0.1)
|
||||||
|
|
||||||
|
vertices.push_back(Vector3(b1.x,b_post.y,b1.y))
|
||||||
|
vertices.push_back(b_post)
|
||||||
|
|
||||||
|
vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
|
||||||
|
# 初始化 ArrayMesh。
|
||||||
|
var arr_mesh = ArrayMesh.new()
|
||||||
|
var arrays = []
|
||||||
|
arrays.resize(Mesh.ARRAY_MAX)
|
||||||
|
arrays[Mesh.ARRAY_VERTEX] = vertices
|
||||||
|
# 创建 Mesh。
|
||||||
|
arr_mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arrays)
|
||||||
|
node.mesh = arr_mesh
|
||||||
|
node.mesh.surface_set_material(0,preload("res://line.tres"))
|
||||||
|
pass
|
||||||
|
|
||||||
|
func calculate_A2_position(A1: Vector2, B1: Vector2, distance_to_A2: float) -> Vector2:
|
||||||
|
var m_l2 = (B1.y - A1.y) / (B1.x - A1.x) # 计算m_l2 (B1 和 A1之间的斜率)
|
||||||
|
var m_l1 = -1 / m_l2 # 计算m_l1 (垂直线的斜率)
|
||||||
|
var dx = distance_to_A2 / sqrt(1 + pow(m_l1, 2)) # 计算 dx 和 dy
|
||||||
|
var dy = m_l1 * dx
|
||||||
|
# 计算 A2 的位置 (有两个可能的解)
|
||||||
|
var A2_1 = Vector2(A1.x + dx, A1.y + dy)
|
||||||
|
var A2_2 = Vector2(A1.x - dx, A1.y - dy)
|
||||||
|
|
||||||
|
var direction_to_b1 = (B1 - A1).normalized()
|
||||||
|
if direction_to_b1.cross(A2_1 - A1) > 0:
|
||||||
|
return A2_1
|
||||||
|
else:
|
||||||
|
return A2_2
|
||||||
|
|
||||||
|
# 可视化矢量地图
|
||||||
|
func update_vector_map_marker():
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/map/vector_map_marker",
|
||||||
|
"type": "visualization_msgs/msg/MarkerArray"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(var_to_str(send_data))
|
||||||
|
|
||||||
|
# 可视化障碍物
|
||||||
|
func update_dynamic_object_marker():
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/perception/object_recognition/objects",
|
||||||
|
"type": "autoware_auto_perception_msgs/msg/PredictedObjects"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(var_to_str(send_data))
|
||||||
|
|
||||||
|
|
||||||
|
# path drivable area marker
|
||||||
|
func update_path_drivable_area_marker():
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/scenario_planning/lane_driving/behavior_planning/path",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Path"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(var_to_str(send_data))
|
||||||
|
|
||||||
|
|
||||||
|
# trajectory marker
|
||||||
|
func update_trajectory_marker():
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/scenario_planning/trajectory",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Trajectory"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(var_to_str(send_data))
|
||||||
|
|
||||||
|
# add ptah left_line and right_line
|
||||||
|
func update_global_path_traiectory():
|
||||||
|
var send_data = {
|
||||||
|
"op": "subscribe",
|
||||||
|
"topic": "/planning/scenario_planning/lane_driving/behavior_planning/path",
|
||||||
|
"type": "autoware_auto_planning_msgs/msg/Path"
|
||||||
|
}
|
||||||
|
|
||||||
|
Websocket.send_msg(var_to_str(send_data))
|
||||||
|
|
||||||
|
|
||||||
|
# ROS 到 Godot 的转换函数
|
||||||
|
func ros2_to_godot(x, y, z) -> Vector3:
|
||||||
|
# 将 ROS 的坐标 (x, y, z) 转换为 Godot 的坐标 (x, z, -y)
|
||||||
|
return Vector3(x, z, -y)
|
17
app/src/main/assets/scripts/test16.gd
Normal file
17
app/src/main/assets/scripts/test16.gd
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
@tool
|
||||||
|
extends Node3D
|
||||||
|
@export var a = false:
|
||||||
|
set(value):
|
||||||
|
var data = {}
|
||||||
|
var vertices = []
|
||||||
|
for point in data.msg.points:
|
||||||
|
var post = (point.pose.position)
|
||||||
|
vertices.push_back(Vector3(post.x, post.z, -post.y))
|
||||||
|
pass
|
||||||
|
add_ptah(vertices,$MeshInstance3D)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
194
app/src/main/assets/scripts/traffic_light_mesh.gd
Normal file
194
app/src/main/assets/scripts/traffic_light_mesh.gd
Normal file
@ -0,0 +1,194 @@
|
|||||||
|
|
||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var traffic_light_recognition = TrafficLights.new()
|
||||||
|
var traffic_light_group_map: Array
|
||||||
|
|
||||||
|
@export var light_bulb_offset = 0.0
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
traffic_light_recognition.subscribe("/perception/traffic_light_recognition/traffic_signals", false)
|
||||||
|
|
||||||
|
# Function to generate a light bulb, setting up its mesh and material
|
||||||
|
func generate_light_bulb(light_bulb) -> MeshInstance3D:
|
||||||
|
var plane_mesh = PlaneMesh.new()
|
||||||
|
plane_mesh.size = Vector2(light_bulb["radius"] * 2, light_bulb["radius"] * 2)
|
||||||
|
|
||||||
|
# Creating mesh instance
|
||||||
|
var mesh_instance = MeshInstance3D.new()
|
||||||
|
|
||||||
|
# Creating ArrayMesh
|
||||||
|
var array_mesh = ArrayMesh.new()
|
||||||
|
array_mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, plane_mesh.get_mesh_arrays())
|
||||||
|
|
||||||
|
# Setting material
|
||||||
|
var material = StandardMaterial3D.new()
|
||||||
|
material.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
|
||||||
|
material.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
|
||||||
|
if light_bulb["color"] == "red":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_red.png")
|
||||||
|
elif light_bulb["color"] == "yellow":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_yellow.png")
|
||||||
|
elif light_bulb["color"] == "green":
|
||||||
|
if light_bulb["arrow"] == "none":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_green.png")
|
||||||
|
elif light_bulb["arrow"] == "right":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_green_right.png")
|
||||||
|
elif light_bulb["arrow"] == "left":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_green_left.png")
|
||||||
|
elif light_bulb["arrow"] == "up":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_green_up.png")
|
||||||
|
elif light_bulb["arrow"] == "down":
|
||||||
|
material.albedo_texture = load("res://TrafficLight/traffic_light_green_down.png")
|
||||||
|
mesh_instance.material_override = material
|
||||||
|
|
||||||
|
# Setting mesh
|
||||||
|
mesh_instance.mesh = array_mesh
|
||||||
|
|
||||||
|
# Setting mesh instance transformation and position
|
||||||
|
var target_normal = light_bulb["normal"].normalized()
|
||||||
|
var angle_y = atan2(target_normal.x, target_normal.z)
|
||||||
|
var rotation_y = Transform3D(Basis().rotated(Vector3.UP, angle_y), Vector3.ZERO)
|
||||||
|
var angle_z = atan2(target_normal.y, Vector2(target_normal.x, target_normal.z).length()) + PI * 0.5
|
||||||
|
var rotation_z = Transform3D(Basis().rotated(Vector3.LEFT, angle_z), Vector3.ZERO)
|
||||||
|
var rotation_flip = Transform3D(Basis().rotated(Vector3.UP, PI), Vector3.ZERO)
|
||||||
|
|
||||||
|
# Applying final transformation
|
||||||
|
mesh_instance.transform = Transform3D().translated(light_bulb["position"]) * rotation_y * rotation_z * rotation_flip * Transform3D().translated(Vector3(0,light_bulb_offset,0))
|
||||||
|
|
||||||
|
return mesh_instance
|
||||||
|
|
||||||
|
func set_traffic_light_map(traffic_light_group_list):
|
||||||
|
traffic_light_group_map = traffic_light_group_list
|
||||||
|
|
||||||
|
# Function to generate the visuals for light bulbs
|
||||||
|
func visualize():
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
|
||||||
|
var boards_arr = []
|
||||||
|
boards_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var boards_verts = PackedVector3Array()
|
||||||
|
var boards_normals = PackedVector3Array()
|
||||||
|
var boards_colors = PackedColorArray()
|
||||||
|
|
||||||
|
# Boards
|
||||||
|
for traffic_light_group in traffic_light_group_map:
|
||||||
|
for traffic_light in traffic_light_group["traffic_lights"]:
|
||||||
|
boards_verts.append(traffic_light["board"]["left_top_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
boards_verts.append(traffic_light["board"]["right_top_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
boards_verts.append(traffic_light["board"]["right_bottom_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
|
||||||
|
boards_verts.append(traffic_light["board"]["left_bottom_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
boards_verts.append(traffic_light["board"]["left_top_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
boards_verts.append(traffic_light["board"]["right_bottom_position"])
|
||||||
|
boards_normals.append(traffic_light["board"]["normal"])
|
||||||
|
boards_colors.append(Color(0.1, 0.1, 0.1, 1.0))
|
||||||
|
|
||||||
|
boards_arr[Mesh.ARRAY_VERTEX] = boards_verts
|
||||||
|
boards_arr[Mesh.ARRAY_NORMAL] = boards_normals
|
||||||
|
boards_arr[Mesh.ARRAY_COLOR] = boards_colors
|
||||||
|
|
||||||
|
# Light Bulbs
|
||||||
|
var light_bulbs_arr = []
|
||||||
|
light_bulbs_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var light_bulbs_verts = PackedVector3Array()
|
||||||
|
var light_bulbs_normals = PackedVector3Array()
|
||||||
|
var light_bulbs_colors = PackedColorArray()
|
||||||
|
|
||||||
|
var segments = 16 # Number of segments to form a circle. Increase this number for a smoother circle.
|
||||||
|
var angle_step = 2.0 * PI / segments # Angle per segment
|
||||||
|
for traffic_light_group in traffic_light_group_map:
|
||||||
|
for traffic_light in traffic_light_group["traffic_lights"]:
|
||||||
|
for light_bulb in traffic_light["light_bulbs"]:
|
||||||
|
var center = light_bulb["position"] # Center of the circle
|
||||||
|
var radius = light_bulb["radius"] # Radius of the circle
|
||||||
|
var normal = light_bulb["normal"].normalized() # Normal vector of the circle
|
||||||
|
var angle_y = atan2(normal.x, normal.z)
|
||||||
|
var rotation_y = Transform3D(Basis().rotated(Vector3.UP, angle_y), Vector3.ZERO)
|
||||||
|
var angle_z = atan2(normal.y, Vector2(normal.x, normal.z).length()) + PI * 0.5
|
||||||
|
var rotation_z = Transform3D(Basis().rotated(Vector3.LEFT, angle_z), Vector3.ZERO)
|
||||||
|
for i in range(segments):
|
||||||
|
# Calculate two points (current point and next point)
|
||||||
|
var current_angle = angle_step * i
|
||||||
|
var next_angle = angle_step * (i + 1)
|
||||||
|
var current_point_local = Vector3(cos(current_angle), 0.0, sin(current_angle)) * radius
|
||||||
|
var next_point_local = Vector3(cos(next_angle), 0.0, sin(next_angle)) * radius
|
||||||
|
|
||||||
|
# Apply rotation and transform to world coordinates
|
||||||
|
var current_point = center + rotation_y * rotation_z * current_point_local
|
||||||
|
var next_point = center + rotation_y * rotation_z * next_point_local
|
||||||
|
|
||||||
|
# Add vertices
|
||||||
|
light_bulbs_verts.append(center)
|
||||||
|
light_bulbs_verts.append(current_point)
|
||||||
|
light_bulbs_verts.append(next_point)
|
||||||
|
|
||||||
|
# Add normals
|
||||||
|
light_bulbs_normals.append(normal)
|
||||||
|
light_bulbs_normals.append(normal)
|
||||||
|
light_bulbs_normals.append(normal)
|
||||||
|
|
||||||
|
# Add colors
|
||||||
|
light_bulbs_colors.append(Color(0.0, 0.0, 0.0, 1.0))
|
||||||
|
light_bulbs_colors.append(Color(0.0, 0.0, 0.0, 1.0))
|
||||||
|
light_bulbs_colors.append(Color(0.0, 0.0, 0.0, 1.0))
|
||||||
|
|
||||||
|
light_bulbs_arr[Mesh.ARRAY_VERTEX] = light_bulbs_verts
|
||||||
|
light_bulbs_arr[Mesh.ARRAY_NORMAL] = light_bulbs_normals
|
||||||
|
light_bulbs_arr[Mesh.ARRAY_COLOR] = light_bulbs_colors
|
||||||
|
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, light_bulbs_arr)
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, boards_arr)
|
||||||
|
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if !traffic_light_recognition.has_new():
|
||||||
|
return
|
||||||
|
|
||||||
|
var traffic_light_status_list = traffic_light_recognition.get_traffic_light_status()
|
||||||
|
|
||||||
|
# Dictionary to mark existing meshes
|
||||||
|
var existing_meshes = {}
|
||||||
|
for child in get_children():
|
||||||
|
if child is MeshInstance3D:
|
||||||
|
existing_meshes[child.name] = child
|
||||||
|
|
||||||
|
for traffic_light_group in traffic_light_group_map:
|
||||||
|
for traffic_light_status in traffic_light_status_list:
|
||||||
|
if traffic_light_status["group_id"] != traffic_light_group["group_id"]:
|
||||||
|
continue
|
||||||
|
|
||||||
|
for traffic_light_status_element in traffic_light_status["status_elements"]:
|
||||||
|
|
||||||
|
for traffic_light in traffic_light_group["traffic_lights"]:
|
||||||
|
for light_bulb in traffic_light["light_bulbs"]:
|
||||||
|
if light_bulb["color"] != traffic_light_status_element["color"]:
|
||||||
|
continue
|
||||||
|
if light_bulb["arrow"] != traffic_light_status_element["arrow"]:
|
||||||
|
continue
|
||||||
|
var mesh_name = "Mesh_" + str(traffic_light_status["group_id"]) + "_" + light_bulb["color"] + "_" + light_bulb["arrow"]
|
||||||
|
var mesh_instance
|
||||||
|
if mesh_name in existing_meshes:
|
||||||
|
mesh_instance = existing_meshes[mesh_name]
|
||||||
|
else:
|
||||||
|
mesh_instance = generate_light_bulb(light_bulb)
|
||||||
|
mesh_instance.name = mesh_name
|
||||||
|
add_child(mesh_instance)
|
||||||
|
# Exclude this mesh from the deletion list since it's being reused
|
||||||
|
existing_meshes.erase(mesh_name)
|
||||||
|
|
||||||
|
# Delete meshes that were not used
|
||||||
|
for mesh_instance in existing_meshes.values():
|
||||||
|
remove_child(mesh_instance)
|
||||||
|
mesh_instance.queue_free()
|
||||||
|
traffic_light_recognition.set_old()
|
@ -0,0 +1,43 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var traffic_light_markers = MarkerArray.new()
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
traffic_light_markers.subscribe("/perception/traffic_light_recognition/traffic_signals/markers", false)
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if !traffic_light_markers.has_new():
|
||||||
|
return
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
|
||||||
|
var traffic_lights = traffic_light_markers.get_color_spheres("traffic_light")
|
||||||
|
var spheres_arr = []
|
||||||
|
var sphere_verts = PackedVector3Array()
|
||||||
|
var sphere_normals = PackedVector3Array()
|
||||||
|
var sphere_indices = PackedInt32Array()
|
||||||
|
var sphere_colors = PackedColorArray()
|
||||||
|
spheres_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
|
||||||
|
var index_offset = 0
|
||||||
|
for traffic_light in traffic_lights:
|
||||||
|
var sphere_mesh = SphereMesh.new()
|
||||||
|
sphere_mesh.set_height(traffic_light[3].x)
|
||||||
|
sphere_mesh.set_radius(traffic_light[3].x * 0.5)
|
||||||
|
sphere_mesh.set_radial_segments(8)
|
||||||
|
sphere_mesh.set_rings(4)
|
||||||
|
|
||||||
|
var sphere_arr = sphere_mesh.get_mesh_arrays()
|
||||||
|
for sphere_vert in sphere_arr[Mesh.ARRAY_VERTEX]:
|
||||||
|
sphere_verts.append(sphere_vert + Vector3(traffic_light[1].x, traffic_light[1].y, traffic_light[1].z))
|
||||||
|
sphere_colors.append(Color(traffic_light[0].r, traffic_light[0].g, traffic_light[0].b, 1.0))
|
||||||
|
for sphere_normal in sphere_arr[Mesh.ARRAY_NORMAL]:
|
||||||
|
sphere_normals.append(sphere_normal)
|
||||||
|
for sphere_index in sphere_arr[Mesh.ARRAY_INDEX]:
|
||||||
|
sphere_indices.append(sphere_index+index_offset)
|
||||||
|
index_offset += sphere_arr[Mesh.ARRAY_VERTEX].size()
|
||||||
|
spheres_arr[Mesh.ARRAY_VERTEX] = sphere_verts
|
||||||
|
spheres_arr[Mesh.ARRAY_NORMAL] = sphere_normals
|
||||||
|
spheres_arr[Mesh.ARRAY_INDEX] = sphere_indices
|
||||||
|
spheres_arr[Mesh.ARRAY_COLOR] = sphere_colors
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, spheres_arr)
|
||||||
|
traffic_light_markers.set_old()
|
66
app/src/main/assets/scripts/trajectory_mesh.gd
Normal file
66
app/src/main/assets/scripts/trajectory_mesh.gd
Normal file
@ -0,0 +1,66 @@
|
|||||||
|
extends MeshInstance3D
|
||||||
|
|
||||||
|
var trajectory = Trajectory.new()
|
||||||
|
@export var wheelbase_to_front: float = 3.78
|
||||||
|
|
||||||
|
func velocity_to_normalized_value(velocity):
|
||||||
|
return clamp(velocity / 5.0, 0.0, 1.0)
|
||||||
|
func velocity_to_color(velocity):
|
||||||
|
var alpha = clamp(velocity_to_normalized_value(velocity), 0.2, 0.9)
|
||||||
|
return Color(0, 0.2, 1.0, alpha)
|
||||||
|
|
||||||
|
func _ready():
|
||||||
|
trajectory.subscribe("/planning/scenario_planning/trajectory", false)
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if !trajectory.has_new():
|
||||||
|
return
|
||||||
|
|
||||||
|
# Trajectory
|
||||||
|
var traj_arr = []
|
||||||
|
traj_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var traj_verts = PackedVector3Array()
|
||||||
|
#var traj_uvs = PackedVector2Array()
|
||||||
|
var traj_normals = PackedVector3Array()
|
||||||
|
#var traj_indices = PackedInt32Array()
|
||||||
|
var traj_colors = PackedColorArray()
|
||||||
|
# Create triangle
|
||||||
|
var trajectory_triangle_strip = trajectory.get_trajectory_triangle_strip(2.0)
|
||||||
|
for point in trajectory_triangle_strip:
|
||||||
|
traj_verts.append(point["position"])
|
||||||
|
traj_normals.append(point["normal"])
|
||||||
|
#traj_uvs.append(Vector2(velocity_to_normalized_value(point["velocity"]), 0))
|
||||||
|
traj_colors.append(Color(0.0, 0.02, 1.0, 0.8))
|
||||||
|
traj_arr[Mesh.ARRAY_VERTEX] = traj_verts
|
||||||
|
traj_arr[Mesh.ARRAY_NORMAL] = traj_normals
|
||||||
|
traj_arr[Mesh.ARRAY_COLOR] = traj_colors
|
||||||
|
# traj_arr[Mesh.ARRAY_INDEX] = traj_indices
|
||||||
|
# traj_arr[Mesh.ARRAY_TEX_UV] = traj_uvs
|
||||||
|
|
||||||
|
|
||||||
|
# Wall
|
||||||
|
var wall_arr = []
|
||||||
|
wall_arr.resize(Mesh.ARRAY_MAX)
|
||||||
|
var wall_verts = PackedVector3Array()
|
||||||
|
#var wall_uvs = PackedVector2Array()
|
||||||
|
var wall_normals = PackedVector3Array()
|
||||||
|
#var wall_indices = PackedInt32Array()
|
||||||
|
var wall_colors = PackedColorArray()
|
||||||
|
# Create triangle
|
||||||
|
var wall_triangle_strip = trajectory.get_wall_triangle_strip(4.0, 2.0, wheelbase_to_front, true, true)
|
||||||
|
for point in wall_triangle_strip:
|
||||||
|
wall_verts.append(point["position"])
|
||||||
|
wall_normals.append(point["normal"])
|
||||||
|
wall_colors.append(point["color"])
|
||||||
|
wall_arr[Mesh.ARRAY_VERTEX] = wall_verts
|
||||||
|
wall_arr[Mesh.ARRAY_NORMAL] = wall_normals
|
||||||
|
wall_arr[Mesh.ARRAY_COLOR] = wall_colors
|
||||||
|
|
||||||
|
if !traj_verts.is_empty():
|
||||||
|
mesh.clear_surfaces()
|
||||||
|
# Trajectory
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, traj_arr)
|
||||||
|
# Wall
|
||||||
|
if !wall_triangle_strip.is_empty():
|
||||||
|
mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLE_STRIP, wall_arr)
|
||||||
|
trajectory.set_old()
|
44
app/src/main/assets/scripts/vector_map.gd
Normal file
44
app/src/main/assets/scripts/vector_map.gd
Normal file
@ -0,0 +1,44 @@
|
|||||||
|
|
||||||
|
extends Node3D
|
||||||
|
|
||||||
|
var vector_map = VectorMap.new()
|
||||||
|
@export var a = false:
|
||||||
|
set(value):
|
||||||
|
vector_map.subscribe("/map/vector_map", true)
|
||||||
|
|
||||||
|
#func _ready():
|
||||||
|
#vector_map.subscribe("/map/vector_map", true)
|
||||||
|
|
||||||
|
|
||||||
|
func _process(_delta):
|
||||||
|
if !vector_map.has_new():
|
||||||
|
return
|
||||||
|
if !vector_map.generate_graph_structure():
|
||||||
|
return
|
||||||
|
# Road Surface
|
||||||
|
var road_surface = get_node("RoadSurfaceMesh")
|
||||||
|
var road_surface_triangle_list = Array()
|
||||||
|
|
||||||
|
road_surface_triangle_list.append_array(vector_map.get_lanelet_triangle_list("road"))
|
||||||
|
road_surface_triangle_list.append_array(vector_map.get_lanelet_triangle_list("shoulder"))
|
||||||
|
#road_surface_triangle_list.append_array(vector_map.get_lanelet_triangle_list("crosswalk"))
|
||||||
|
#road_surface_triangle_list.append_array(vector_map.get_lanelet_triangle_list("walkway"))
|
||||||
|
road_surface_triangle_list.append_array(vector_map.get_polygon_triangle_list("intersection_area"))
|
||||||
|
road_surface_triangle_list.append_array(vector_map.get_polygon_triangle_list("hatched_road_markings_area"))
|
||||||
|
road_surface_triangle_list.append_array(vector_map.get_polygon_triangle_list("parking_lots"))
|
||||||
|
road_surface.visualize_mesh(road_surface_triangle_list)
|
||||||
|
# Road Marker
|
||||||
|
var road_marker = get_node("RoadMarkerMesh")
|
||||||
|
var road_marker_verts = Array()
|
||||||
|
road_marker_verts.append_array(vector_map.get_polygon_triangle_list("pedestrian_marking"))
|
||||||
|
road_marker_verts.append_array(vector_map.get_linestring_triangle_list("shared_white_line", 0.05))
|
||||||
|
#road_marker_verts.append_array(vector_map.get_linestring_triangle_list("white_line", 0.05))
|
||||||
|
road_marker_verts.append_array(vector_map.get_linestring_triangle_list("stop_line", 0.5))
|
||||||
|
road_marker.visualize_mesh(road_marker_verts)
|
||||||
|
# Traffic Light
|
||||||
|
var traffic_light = get_node("TrafficLightMesh")
|
||||||
|
traffic_light.set_traffic_light_map(vector_map.get_traffic_light_list())
|
||||||
|
#traffic_light.visualize_mesh(vector_map.get_triangle_list("traffic_light_triangle"), vector_map.get_color_spheres("traffic_light"))
|
||||||
|
traffic_light.visualize()
|
||||||
|
|
||||||
|
vector_map.set_old()
|
Loading…
Reference in New Issue
Block a user