157 lines
6.4 KiB
GDScript
157 lines
6.4 KiB
GDScript
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)
|