LargeScreen/res3D/ego_vehicle.gd

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)