372 lines
11 KiB
GDScript3
372 lines
11 KiB
GDScript3
|
#@tool
|
||
|
|
extends Node3D
|
||
|
|
|
||
|
|
# 模拟对象分类标签的常量
|
||
|
|
enum label {
|
||
|
|
UNKNOWN,
|
||
|
|
CAR,
|
||
|
|
TRUCK,
|
||
|
|
BUS,
|
||
|
|
TRAILER,
|
||
|
|
MOTORCYCLE,
|
||
|
|
BICYCLE,
|
||
|
|
PEDESTRIAN
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
# 定义不同类型对象的模型池，包含预加载的场景资源和对象池
|
||
|
|
var model_types = {
|
||
|
|
"car": {"scene": preload("res://Dynamic_Object/car.tscn"), "pool": []},
|
||
|
|
"pedestrian": {"scene": preload("res://Dynamic_Object/pedestrian.tscn"), "pool": []},
|
||
|
|
"truck": {"scene": preload("res://Dynamic_Object/suv.tscn"), "pool": []},
|
||
|
|
"trailer": {"scene": preload("res://Dynamic_Object/trailer.tscn"), "pool": []},
|
||
|
|
"bus": {"scene": preload("res://Dynamic_Object/bus.tscn"), "pool": []},
|
||
|
|
"bicycle": {"scene": preload("res://Dynamic_Object/bicycle.tscn"), "pool": []},
|
||
|
|
"motorcycle": {"scene": preload("res://Dynamic_Object/motorcycle.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://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"
|
||
|
|
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.mb_post = (Vector3(obj["position"].x, obj["position"].y - obj["size"].y * 0.5, obj["position"].z))
|
||
|
|
node.mb_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)
|