extends Node3D @onready var view_camera: Camera3D = %ViewCamera @onready var side_display_sub_viewport: SubViewport = $SideDisplaySubViewport @onready var main_sub_viewport: SubViewport = $MainSubViewport @onready var vertical: Node3D = %Vertical @onready var camera_3d: Node3D = %Camera3D func _init() -> void: Global.EgoVehicle3D = self func _ready() -> void: Websocket.connected_to_server.connect(connected_to_server) Websocket.message_received.connect(message_received) func SideDisplayGetTexture(): return side_display_sub_viewport.get_texture() func MainSubGetTexture(): return main_sub_viewport.get_texture() func connected_to_server(): pose() pass var actuation_status = 0 var mb_post = Vector3(0,0,0) var mb_rotation = Vector3(0,0,0) var js = 360 func message_received(data: Dictionary): if data.is_empty(): return if !data.has("topic"):return match data.topic: "/hmi_input/app/cloud_control_platform/vehicle_info": if data.msg.head_light == 1: %polySurface14.set("surface_material_override/0",preload("res://Headlight.tres")) %polySurface15.set("surface_material_override/0",preload("res://Headlight.tres")) %polySurface16.set("surface_material_override/0",preload("res://Headlight.tres")) %polySurface17.set("surface_material_override/0",preload("res://Headlight.tres")) %polySurface26.set("surface_material_override/0",preload("res://Headlight.tres")) %polySurface30.set("surface_material_override/0",preload("res://Headlight.tres")) else: %polySurface14.set("surface_material_override/0",null) %polySurface15.set("surface_material_override/0",null) %polySurface16.set("surface_material_override/0",null) %polySurface17.set("surface_material_override/0",null) %polySurface26.set("surface_material_override/0",null) %polySurface30.set("surface_material_override/0",null) #转向灯 if data.msg.turn_light == 1: %polySurface14.set("surface_material_override/0",preload("res://TurnSignal.tres")) %polySurface15.set("surface_material_override/0",preload("res://TurnSignal.tres")) %polySurface16.set("surface_material_override/0",null) %polySurface17.set("surface_material_override/0",null) elif data.msg.turn_light == 2: %polySurface16.set("surface_material_override/0",preload("res://TurnSignal.tres")) %polySurface17.set("surface_material_override/0",preload("res://TurnSignal.tres")) %polySurface14.set("surface_material_override/0",null) %polySurface15.set("surface_material_override/0",null) pass if data.msg.brk>0: %polySurface26.set("surface_material_override/0",preload("res://Brake.tres")) %polySurface30.set("surface_material_override/0",preload("res://Brake.tres")) else: %polySurface26.set("surface_material_override/0",null) %polySurface30.set("surface_material_override/0",null) actuation_status = data.msg.thr "/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) %polySurface31.rotation.y = actual_angle %polySurface33.rotation.y = -actual_angle %Q1_23.rotation.y = actual_angle %Q1_22.rotation.y = actual_angle #$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 var jd = 360 func xz(delta): jd -= delta if jd <= 0: jd = 360 var arr = [%Q1_23, %Q1_17, %Q1_22, %Q1_21] for item in arr: item.rotation_degrees.z = jd * -1 func _process(delta): xz(delta*500*actuation_status) if mb_post.distance_to(global_position) > 10: global_position = mb_post rotation = mb_rotation else: global_position = lerp(global_position,mb_post,1*delta) rotation.x = lerp_angle(rotation.x,mb_rotation.x,1*delta) rotation.y = lerp_angle(rotation.y,mb_rotation.y,1*delta) rotation.z = lerp_angle(rotation.z,mb_rotation.z,1*delta) #xz(delta*500*actuation_status) %Camera3D.global_position = global_position #%Camera3D.global_position.z = global_position.z #%Camera3D.global_position.y = global_position.y %Camera3D.global_rotation_degrees.y = global_rotation_degrees.y - 90 #$SubViewport2/Camera3D.global_transform = %ViewCamera.global_transform func pose():# var send_data = { "op": "subscribe", "topic": "/localization/pose_twist_fusion_filter/pose", "type": "geometry_msgs/msg/PoseStamped" } print("post请求发送") Websocket.send_msg(str(send_data)) # 将四元数转换为欧拉角（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)