95 lines
3.2 KiB
GDScript
95 lines
3.2 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)
|
||
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))
|
||
pass
|
||
func _process(delta):
|
||
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)
|