diff --git a/.github/workflows/build_windows.yml b/.github/workflows/build_windows.yml index 9501b3feee..6d6dd9349d 100644 --- a/.github/workflows/build_windows.yml +++ b/.github/workflows/build_windows.yml @@ -53,6 +53,7 @@ jobs: make check.smoke make check.air make check.vr + make check.water make check.python shell: cmd diff --git a/.gitignore b/.gitignore index d6cd6a6203..6333fa045a 100644 --- a/.gitignore +++ b/.gitignore @@ -80,4 +80,21 @@ profiler.csv .venv devcontainer.json CarlaUE4.xcworkspace -OptionalModules.ini \ No newline at end of file +OptionalModules.ini +CesiumForUnreal* +Unreal/CarlaUE4/Plugins/AirSim/ + +# --- 水下仿真整合(2026-11)--- +# HoloOcean 八叉树运行时缓存,由插件在运行时生成 +Unreal/CarlaUE4/Octrees/ +# Cook 阶段生成的材质清单 +materials.csv +# 修改 .uproject 前留的备份 +*.uproject.bak* +# 本地调参用的 AirSim 配置副本(正式配置在 Unreal//settings.json) +/settings.json +/settings_rov.json + +# OceanPlugin(UE4.26 原生海洋/水下材质库,234 MB) +# 体积大且是第三方仓库,不入库;用 Unreal/CarlaUE4/Plugins/setup_ocean_plugin.ps1 拉取 +Unreal/CarlaUE4/Plugins/OceanPlugin/ diff --git a/CarlaAir.ps1 b/CarlaAir.ps1 index e7b6884030..df944b2aaf 100644 --- a/CarlaAir.ps1 +++ b/CarlaAir.ps1 @@ -1,10 +1,48 @@ +<# +.SYNOPSIS + CarlaAir 联合仿真一键启动与自动化调试器 (Windows Launcher) + +.DESCRIPTION + 本脚本是面向开发者在本地开发与算法调试阶段的一键启动工具,用于自动化拉起 CARLA 与 AirSim 联合仿真环境。 + + 【与 StartOpenHUTB.bat / CarlaUE4.exe 的区别与工作流定位】: + - StartOpenHUTB.bat / CarlaUE4.exe: + 面向最终发布/打包(Package)版本用户的标准启动方式。主要用于部署后直接运行打包好的二进制文件,并自动检查 VC++ 运行库。 + - CarlaAir.ps1: + 面向开发人员与算法测试人员的 PowerShell 自动化启动脚本。具备以下高级特性: + 1. 自动定位本地 Carla 可执行文件(支持 Editor 模式、开发构建与 WindowsNoEditor 打包模式); + 2. 灵活指定仿真地图(默认 Town10HD)、窗口分辨率、画质等级; + 3. 支持载具视角快速切换(通过 --vehicle 参数在空中无人机 drone 与水下机器人 rov 之间自动切换); + 4. 自动分发对应的 AirSim 配置文件到 ~/Documents/AirSim/settings.json; + 5. 自动检测 CARLA (2000) 和 AirSim (41451) 端口连通性,并在就绪后自动拉起背景交通流进程 (auto_traffic.py); + 6. 支持实例健康监控、日志追踪 (--log) 与一键清理停止 (--kill)。 + +.EXAMPLE + # 启动水下机器人 ROV 仿真(默认地图 Town10HD) + .\CarlaAir.ps1 --vehicle rov + + # 启动空中多旋翼无人机仿真 + .\CarlaAir.ps1 --vehicle drone + + # 在 Town10HD 上启动 ROV,禁用背景交通流,并挂接控制台查看输出 + .\CarlaAir.ps1 Town10HD --vehicle rov --no-traffic --fg + + # 停止当前运行的 CarlaAir 实例 + .\CarlaAir.ps1 --kill +#> + $ErrorActionPreference = "Stop" function Show-Usage { @" Usage: .\CarlaAir.ps1 [MAP] [OPTIONS] +Workflow Note: + - For packaged end-user deployments: Use StartOpenHUTB.bat or CarlaUE4.exe. + - For development & automated co-simulation testing: Use .\CarlaAir.ps1. + Options: + --vehicle TYPE Vehicle type to spawn: 'rov' (default) or 'drone' --res WxH Window resolution (default: 1280x720) --port PORT CARLA RPC port (default: 2000) --quality LEVEL Quality level: Low, Medium, High, Epic @@ -149,6 +187,25 @@ function Resolve-CarlaBinary { } } + $editorCandidates = @() + if ($env:UE4_ROOT) { + $p = Join-Path $env:UE4_ROOT "Engine\Binaries\Win64\UE4Editor.exe" + if (Test-Path $p) { $editorCandidates += $p } + } + $defaultEditor = "E:\Projects\hutb_editor\hutb_editor\unreal\Engine\Binaries\Win64\UE4Editor.exe" + if (Test-Path $defaultEditor) { $editorCandidates += $defaultEditor } + + $uproject = Join-Path $RepoRoot "Unreal\CarlaUE4\CarlaUE4.uproject" + if (($editorCandidates.Count -gt 0) -and (Test-Path $uproject)) { + return @{ + Binary = (Resolve-Path $editorCandidates[0]).Path + WorkingDirectory = Split-Path -Parent $uproject + NeedsProjectArg = $false + IsEditor = $true + ProjectFile = (Resolve-Path $uproject).Path + } + } + throw "No Windows Carla binary found. Build the project first with .\BuildWindows.ps1." } @@ -221,6 +278,7 @@ function Stop-CarlaAirProcess { $repoRoot = Split-Path -Parent $MyInvocation.MyCommand.Path $mapName = "Town10HD" +$vehicleType = "rov" $resX = 1280 $resY = 720 $carlaPort = 2000 @@ -239,6 +297,7 @@ for ($i = 0; $i -lt $args.Count; $i++) { $arg = [string]$args[$i] switch -Regex ($arg) { '^--help$|^-h$' { Show-Usage; exit 0 } + '^--vehicle$' { $i++; $vehicleType = ([string]$args[$i]).ToLower(); continue } '^--fg$' { $foreground = $true; continue } '^--kill$' { $killOnly = $true; continue } '^--log$' { $showLog = $true; continue } @@ -287,23 +346,63 @@ if ($showLog) { $binaryInfo = Resolve-CarlaBinary -RepoRoot $repoRoot -ExplicitPackageRoot $explicitPackageRoot $trafficPython = if ($autoTraffic) { Resolve-TrafficPython -ExplicitPath $explicitPython } else { $null } -$airsimSettingsSource = Join-Path $repoRoot "AirSimConfig\settings.json" -if (-not (Test-Path $airsimSettingsSource)) { - throw "AirSim settings template not found: $airsimSettingsSource" +$settingsFileName = if ($vehicleType -eq "drone" -or $vehicleType -eq "multirotor") { + "settings_drone.json" +} else { + "settings_rov.json" +} + +$candidateSources = @( + (Join-Path $repoRoot "Unreal\CarlaUE4\$settingsFileName"), + (Join-Path $repoRoot $settingsFileName), + (Join-Path $repoRoot "AirSimConfig\$settingsFileName") +) + +$airsimSettingsSource = $null +foreach ($cand in $candidateSources) { + if (Test-Path $cand) { + $airsimSettingsSource = $cand + break + } +} + +if (-not $airsimSettingsSource) { + $fallbackSources = @( + (Join-Path $repoRoot "Unreal\CarlaUE4\settings.json"), + (Join-Path $repoRoot "settings.json"), + (Join-Path $repoRoot "AirSimConfig\settings.json") + ) + foreach ($fb in $fallbackSources) { + if (Test-Path $fb) { + $airsimSettingsSource = $fb + break + } + } +} + +if (-not $airsimSettingsSource) { + throw "AirSim settings template not found: $settingsFileName. Checked paths: $($candidateSources -join ', ')" } $airsimSettingsDir = Join-Path $env:USERPROFILE "Documents\AirSim" $airsimSettingsTarget = Join-Path $airsimSettingsDir "settings.json" New-Item -ItemType Directory -Force -Path $airsimSettingsDir | Out-Null Copy-Item $airsimSettingsSource $airsimSettingsTarget -Force +Write-Host "AirSim Settings: Dispatched $settingsFileName -> $airsimSettingsTarget" Stop-CarlaAirProcess -RepoRoot $repoRoot $launchArgs = New-Object System.Collections.Generic.List[string] -if ($binaryInfo.NeedsProjectArg) { +if ($binaryInfo.IsEditor) { + $launchArgs.Add("`"$($binaryInfo.ProjectFile)`"") + $launchArgs.Add($mapName) + $launchArgs.Add("-game") +} elseif ($binaryInfo.NeedsProjectArg) { $launchArgs.Add("CarlaUE4") + $launchArgs.Add($mapName) +} else { + $launchArgs.Add($mapName) } -$launchArgs.Add($mapName) $launchArgs.Add("-windowed") $launchArgs.Add("-ResX=$resX") $launchArgs.Add("-ResY=$resY") @@ -319,6 +418,7 @@ Write-Host "============================================" Write-Host " CarlaAir - Windows Launcher" Write-Host "============================================" Write-Host " Map: $mapName" +Write-Host " Vehicle: $vehicleType ($settingsFileName)" Write-Host " Resolution: ${resX}x${resY}" Write-Host " CARLA Port: $carlaPort" Write-Host " AirSim Port: $airsimPort" @@ -344,7 +444,8 @@ Set-Content -Path $pidFile -Value $process.Id Write-Host "Waiting for CARLA port..." $carlaReady = $false -for ($attempt = 0; $attempt -lt 120; $attempt++) { +$maxAttemptsCarla = if ($binaryInfo.IsEditor) { 900 } else { 120 } +for ($attempt = 0; $attempt -lt $maxAttemptsCarla; $attempt++) { Start-Sleep -Seconds 2 if ($process.HasExited) { throw "CarlaAir exited early. Check $logFile" @@ -354,20 +455,27 @@ for ($attempt = 0; $attempt -lt 120; $attempt++) { $carlaReady = $true break } + if ($attempt % 10 -eq 0 -and $attempt -gt 0) { + Write-Host " Still waiting for CARLA port ($($attempt * 2)s / $($maxAttemptsCarla * 2)s)..." + } } if (-not $carlaReady) { - throw "CARLA port $carlaPort did not become ready within 240 seconds. Check $logFile" + throw "CARLA port $carlaPort did not become ready within $($maxAttemptsCarla * 2) seconds. Check $logFile" } Write-Host "Waiting for AirSim port..." $airsimReady = $false -for ($attempt = 0; $attempt -lt 60; $attempt++) { +$maxAttemptsAirSim = if ($binaryInfo.IsEditor) { 300 } else { 60 } +for ($attempt = 0; $attempt -lt $maxAttemptsAirSim; $attempt++) { Start-Sleep -Seconds 2 if (Test-Port -Port $airsimPort) { Write-Host " AirSim (port $airsimPort): Ready" $airsimReady = $true break } + if ($attempt % 10 -eq 0 -and $attempt -gt 0) { + Write-Host " Still waiting for AirSim port ($($attempt * 2)s / $($maxAttemptsAirSim * 2)s)..." + } } if (-not $airsimReady) { throw "AirSim port $airsimPort did not become ready within 120 seconds. Check $logFile" @@ -377,8 +485,12 @@ if ($autoTraffic) { if (($trafficVehicles -eq 0) -and ($trafficWalkers -eq 0)) { Write-Host "Traffic disabled by count (0 vehicles + 0 walkers)." } elseif ($trafficPython) { + $autoTrafficScript = Join-Path $repoRoot "PythonAPI\examples\air\auto_traffic.py" + if (-not (Test-Path $autoTrafficScript)) { + $autoTrafficScript = Join-Path $repoRoot "auto_traffic.py" + } $trafficArgs = @( - (Join-Path $repoRoot "auto_traffic.py"), + $autoTrafficScript, "--vehicles", [string]$trafficVehicles, "--walkers", [string]$trafficWalkers, "--port", [string]$carlaPort @@ -394,3 +506,7 @@ if ($autoTraffic) { Write-Host "" Write-Host "CarlaAir is ready." Write-Host "Log: $logFile" + +if (-not $foreground) { + Wait-Process -Id $process.Id +} diff --git a/PythonAPI/carla/README.md b/PythonAPI/carla/README.md index 58ac59193a..4e1ef5efd5 100644 --- a/PythonAPI/carla/README.md +++ b/PythonAPI/carla/README.md @@ -1,17 +1,17 @@ -这是 HUTB Python API 的 Python 包,用于控制和与 [HUTB](https://github.com/OpenHUTB/hutb)(用于人车研究的开源模拟器)进行通信。 +这是 OpenHUTB 模拟器 的 Python API 包,用于控制和与 [OpenHUTB](https://github.com/OpenHUTB/hutb)(用于人车研究的开源模拟器)进行通信。 -此包允许您控制 HUTB 模拟器并通过 Python API 检索模拟数据。例如,您可以控制模拟中的任何参与者(人、车、无人机、交通信号灯等),将传感器连接到参与者,并读取传感器数据等。 +此包允许您控制 OpenHUTB 模拟器并通过 Python API 检索模拟数据。例如,您可以控制模拟中的任何参与者(人、车、无人机、交通信号灯等),将传感器连接到参与者,并读取传感器数据等。 ## 开发 ```shell cd hutb/PythonAPI/carla -# 实现编辑模式安装 +# 实现编辑模式安装(注意关闭代理) # 修改 setup.py 后需要再次运行才会生效 pip install -e . # 测试 API python ../util/config.py -l -python ../examples/holoocean/getting_started.py +python ../examples/water/getting_started.py ``` -更多信息,请参阅 [HUTB 文档](https://openhutb.github.io/) 。 +更多信息,请参阅 [OpenHUTB 文档](https://openhutb.github.io/) 。 diff --git a/PythonAPI/carla/source/airsim/client.py b/PythonAPI/carla/source/airsim/client.py index 4f139ac552..bda542b651 100644 --- a/PythonAPI/carla/source/airsim/client.py +++ b/PythonAPI/carla/source/airsim/client.py @@ -1618,4 +1618,104 @@ def getCarControls(self, vehicle_name=''): CarControls: """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) + +#----------------------------------- ROV APIs --------------------------------------------- +class RovClient(VehicleClient, object): + def __init__(self, ip = "", port = 41451, timeout_value = 3600): + super(RovClient, self).__init__(ip, port, timeout_value) + + def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): + return self.client.call_async('takeoff', timeout_sec, vehicle_name) + + def landAsync(self, timeout_sec = 60, vehicle_name = ''): + return self.client.call_async('land', timeout_sec, vehicle_name) + + def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): + return self.client.call_async('goHome', timeout_sec, vehicle_name) + + def hoverAsync(self, vehicle_name = ''): + return self.client.call_async('hover', vehicle_name) + + def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityBodyFrame', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByVelocityZBodyFrameAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityZBodyFrame', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByMotorPWMsAsync(self, pwm_values, duration, vehicle_name = ''): + return self.client.call_async('moveByMotorPWMs', pwm_values, duration, vehicle_name) + + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawZ', roll, pitch, yaw, z, duration, vehicle_name) + + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawThrottle', roll, pitch, yaw, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, pitch, yaw_rate, throttle, duration, vehicle_name) + + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + return self.client.call_async('moveByRollPitchYawrateZ', roll, pitch, yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleRatesZ', roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name) + + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name) + + def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + + def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + + def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, + yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, + yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + + def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, vehicle_name) + + def rotateToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''): + return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name) + + def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''): + return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name) + + def setAngleLevelControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setAngleLevelControllerGains', kp, ki, kd, vehicle_name) + + def setAngleRateControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setAngleRateControllerGains', kp, ki, kd, vehicle_name) + + def setVelocityControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setVelocityControllerGains', kp, ki, kd, vehicle_name) + + def setPositionControllerGains(self, kp, ki, kd, vehicle_name = ''): + self.client.call('setPositionControllerGains', kp, ki, kd, vehicle_name) + + def moveByRC(self, rc_data, vehicle_name = ''): + self.client.call('moveByRC', rc_data, vehicle_name) + + def simSetRovPose(self, pose, tilt_angles, ignore_collision = True, spin_props = False, vehicle_name = ''): + self.client.call('simSetRovPose', pose, tilt_angles, ignore_collision, spin_props, vehicle_name) + + def getRotorStates(self, vehicle_name = ''): + return RotorTiltableStates.from_msgpack(self.client.call('getRotorStates', vehicle_name)) + getRotorStates.__annotations__ = {'return': RotorTiltableStates} + + def getRovState(self, vehicle_name = ''): + return RovState.from_msgpack(self.client.call('getRovState', vehicle_name)) + getRovState.__annotations__ = {'return': RovState} \ No newline at end of file diff --git a/PythonAPI/carla/source/airsim/types.py b/PythonAPI/carla/source/airsim/types.py index 7aef005549..89968d60a0 100644 --- a/PythonAPI/carla/source/airsim/types.py +++ b/PythonAPI/carla/source/airsim/types.py @@ -578,3 +578,25 @@ class MeshPositionVertexBuffersResponse(MsgpackMixin): vertices = 0.0 indices = 0.0 name = '' + +class RotorTiltableParameters(MsgpackMixin): + thrust = 0.0 + torque_scaler = 0.0 + speed = 0.0 + angle = 0.0 + +class RotorTiltableStates(MsgpackMixin): + timestamp = np.uint64(0) + rotors = [] + +class RovState(MsgpackMixin): + collision = CollisionInfo() + kinematics_estimated = KinematicsState() + kinematics_true = KinematicsState() + gps_location = GeoPoint() + timestamp = np.uint64(0) + landed_state = LandedState.Landed + rc_data = RCData() + ready = False + ready_message = "" + can_arm = False diff --git a/PythonAPI/examples/holoocean/teleop_rov.py b/PythonAPI/examples/holoocean/teleop_rov.py new file mode 100644 index 0000000000..274fe15878 --- /dev/null +++ b/PythonAPI/examples/holoocean/teleop_rov.py @@ -0,0 +1,127 @@ +import sys +import time +import os + +# Ensure AirSim python client is in path +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "carla", "source"))) +import airsim +import carla + +def main(): + print("=" * 65) + print(" OpenHUTB 水下机器人 (ROV) 键盘实时遥控器 (Teleop)") + print("=" * 65) + print("正在连接 AirSim 物理引擎与 CARLA 视景服务器...") + + rov = airsim.RovClient() + rov.confirmConnection() + rov.enableApiControl(True) + rov.armDisarm(True) + print("[OK] AirSim ROV 动力学已连接并解锁 (Armed)!") + + carla_client = carla.Client("localhost", 2000) + carla_client.set_timeout(3.0) + world = carla_client.get_world() + spectator = world.get_spectator() + print("[OK] CARLA 视景服务器已连接!") + + # 1. 自动将 ROV 与摄像机瞬移至 Town10 海湾水下 + print("\n[INFO] 正在将 ROV 部署至 Town10 蔚蓝海湾水下 (-2.0m 深度)...") + target_pos = carla.Location(x=150.0, y=100.0, z=-2.0) + + # 查找 airsim drone/rov actor + rov_actor = None + for a in world.get_actors(): + if "airsim" in a.type_id.lower() or "rov" in a.type_id.lower(): + rov_actor = a + break + + if rov_actor: + rov_actor.set_location(target_pos) + cam_loc = carla.Location(x=144.0, y=100.0, z=-0.5) + spectator.set_transform(carla.Transform(cam_loc, carla.Rotation(pitch=-15, yaw=0, roll=0))) + print("[OK] ROV 与摄像机已成功入水!请看虚幻编辑器窗口!") + else: + print("[WARN] 未在 CARLA 中直接匹配到 ROV Actor,将依赖 AirSim 局部坐标。") + + print("\n" + "=" * 65) + print("【操作指引】请在本终端中直接按键操控 ROV (无需回车):") + print(" [W] 前进 (Forward) [S] 后退 (Backward)") + print(" [A] 左转 (Yaw Left) [D] 右转 (Yaw Right)") + print(" [空格] 垂直上浮 (Ascend) [C] 垂直下潜 (Descend)") + print(" [X] 紧急刹车悬停 (Stop) [Q] 退出遥控 (Quit)") + print("=" * 65) + + import msvcrt + + # 推进器配置: + # 0~3: 垂直推进器 + # 4~7: 水平矢量推进器 (4:前右, 5:前左, 6:后左, 7:后右) + while True: + if msvcrt.kbhit(): + ch = msvcrt.getch() + try: + key = ch.decode('utf-8').lower() + except Exception: + continue + + pwms = [0.0] * 8 + action_name = "" + + if key == 'w': + # 前进: 前右推、前左推、后左反推、后右反推 + pwms = [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, -1.0, -1.0] + action_name = "前进 (Forward)" + elif key == 's': + # 后退 + pwms = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, 1.0, 1.0] + action_name = "后退 (Backward)" + elif key == 'a': + # 逆时针左偏航转弯 + pwms = [0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 1.0, -1.0] + action_name = "左转 (Turn Left)" + elif key == 'd': + # 顺时针右偏航转弯 + pwms = [0.0, 0.0, 0.0, 0.0, 1.0, -1.0, -1.0, 1.0] + action_name = "右转 (Turn Right)" + elif key == ' ': + # 垂直上浮 + pwms = [-1.0, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0] + action_name = "上浮 (Ascend)" + elif key == 'c': + # 垂直下潜 + pwms = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0] + action_name = "下潜 (Descend)" + elif key == 'x': + pwms = [0.0] * 8 + action_name = "定深自稳悬停 (Hover)" + elif key == 'q': + print("\n正在释放控制权并退出...") + rov.moveByMotorPWMsAsync([0.0] * 8, 0.1).join() + rov.armDisarm(False) + rov.enableApiControl(False) + print("遥控已安全结束。") + break + else: + continue + + # 发送推进器推力并持续 0.4 秒 + rov.moveByMotorPWMsAsync(pwms, 0.4) + + # 获取状态 + state = rov.getRovState() + pos = state.kinematics_estimated.position + vel = state.kinematics_estimated.linear_velocity + + # 同步摄像机跟随 ROV + if rov_actor: + curr_loc = rov_actor.get_location() + cam_loc = carla.Location(x=curr_loc.x - 5.0, y=curr_loc.y, z=curr_loc.z + 1.5) + spectator.set_transform(carla.Transform(cam_loc, carla.Rotation(pitch=-15, yaw=0, roll=0))) + + print(f"\r[{action_name}] 深度 Z: {pos.z_val:+.2f}m | 速度 Vx: {vel.x_val:+.2f}m/s, Vz: {vel.z_val:+.2f}m/s ", end="", flush=True) + + time.sleep(0.05) + +if __name__ == "__main__": + main() diff --git a/PythonAPI/examples/holoocean/getting_started.py b/PythonAPI/examples/water/getting_started.py similarity index 100% rename from PythonAPI/examples/holoocean/getting_started.py rename to PythonAPI/examples/water/getting_started.py diff --git a/PythonAPI/examples/water/hello_rov.py b/PythonAPI/examples/water/hello_rov.py new file mode 100644 index 0000000000..77ac44c099 --- /dev/null +++ b/PythonAPI/examples/water/hello_rov.py @@ -0,0 +1,59 @@ +import setup_path +import airsim +import time +import pprint + +def main(): + # Connect to the AirSim simulator (ROV) + client = airsim.RovClient() + print("Connecting to AirSim ROV...") + client.confirmConnection() + client.enableApiControl(True) + + print("Arming the ROV...") + client.armDisarm(True) + + # Query vehicle state + state = client.getRovState() + print("ROV State:\n%s" % pprint.pformat(state)) + + # Query sensor states + try: + imu_data = client.getImuData() + print("IMU Data:\n%s" % pprint.pformat(imu_data)) + except Exception as e: + print("IMU query failed: %s" % e) + + try: + baro_data = client.getBarometerData() + print("Barometer (Depth) Data:\n%s" % pprint.pformat(baro_data)) + except Exception as e: + print("Barometer query failed: %s" % e) + + try: + mag_data = client.getMagnetometerData() + print("Magnetometer Data:\n%s" % pprint.pformat(mag_data)) + except Exception as e: + print("Magnetometer query failed: %s" % e) + + # Basic movement test: dive 2 meters and move forward + print("Moving ROV by velocity in body frame (forward 0.5 m/s, dive 0.2 m/s for 3s)...") + client.moveByVelocityBodyFrameAsync(vx=0.5, vy=0.0, vz=0.2, duration=3).join() + + time.sleep(1) + + state = client.getRovState() + print("Updated ROV Position: %s" % state.kinematics_estimated.position) + + # Hover in place + print("Hovering...") + client.hoverAsync().join() + + # Disarm and release control + print("Disarming and resetting...") + client.armDisarm(False) + client.enableApiControl(False) + print("Done!") + +if __name__ == "__main__": + main() diff --git a/PythonAPI/examples/holoocean/installation.py b/PythonAPI/examples/water/installation.py similarity index 100% rename from PythonAPI/examples/holoocean/installation.py rename to PythonAPI/examples/water/installation.py diff --git a/PythonAPI/examples/water/setup_path.py b/PythonAPI/examples/water/setup_path.py new file mode 100644 index 0000000000..4b2e2ec134 --- /dev/null +++ b/PythonAPI/examples/water/setup_path.py @@ -0,0 +1,40 @@ +# Import this module to automatically setup path to local airsim module +import os, sys, logging + +class SetupPath: + @staticmethod + def getDirLevels(path): + path_norm = os.path.normpath(path) + return len(path_norm.split(os.sep)) + + @staticmethod + def getCurrentPath(): + cur_filepath = __file__ + return os.path.dirname(cur_filepath) + + @staticmethod + def getGrandParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 2: + return os.path.dirname(os.path.dirname(cur_path)) + return '' + + @staticmethod + def getParentDir(): + cur_path = SetupPath.getCurrentPath() + if SetupPath.getDirLevels(cur_path) >= 1: + return os.path.dirname(cur_path) + return '' + + @staticmethod + def addAirSimModulePath(): + parent = SetupPath.getParentDir() + if parent != '': + airsim_path = os.path.join(parent, 'airsim') + client_path = os.path.join(airsim_path, 'client.py') + if os.path.exists(client_path): + sys.path.insert(0, parent) + else: + logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).") + +SetupPath.addAirSimModulePath() diff --git a/PythonAPI/test/holoocean/airsim_rov.py b/PythonAPI/test/holoocean/airsim_rov.py new file mode 100644 index 0000000000..e74b2235c1 --- /dev/null +++ b/PythonAPI/test/holoocean/airsim_rov.py @@ -0,0 +1,905 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +airsim_rov.py — Test underwater ROV features in CarlaAir unified simulation +Usage: + python airsim_rov.py [--host 127.0.0.1] [--port 2000] [--airsim-port 41451] + python airsim_rov.py --mock [--test-buoyancy] [--test-depth-clamp] [--all-tests] + +Features & Tests: + 1. CarlaAir settings configuration & Fossen hydrodynamic parameter validation (settings_rov.json) + 2. CARLA actor registry check (airsim.drone / ROV) + 3. AirSim ROV RPC connection & arming (Live or Mock client) + 4. ROV kinematics state & water sensors (IMU, Barometer/Depth, Magnetometer, Downward Sonar) + 5. 6-DOF body-frame velocity control (forward motion & dive) and hover stabilization + 6. Neutral buoyancy balance & attitude self-righting test (--test-buoyancy) + 7. Seabed soft cushion, spring-damper reaction & kinematic hard depth clamp test (--test-depth-clamp) + 8. Underwater stereo camera capture test (1280x720 RGB Scene) + 9. Clean disarm and control release +""" + +import sys +import os +import time +import math +import json +import argparse + +# Force UTF-8 encoding on standard streams in Windows environments +if hasattr(sys.stdout, 'reconfigure'): + try: + sys.stdout.reconfigure(encoding='utf-8') + sys.stderr.reconfigure(encoding='utf-8') + except Exception: + pass + +# Ensure airsim package with RovClient is accessible +airsim_loaded = False +try: + import airsim + if hasattr(airsim, 'RovClient'): + airsim_loaded = True + else: + if 'airsim' in sys.modules: + del sys.modules['airsim'] +except ImportError: + pass + +if not airsim_loaded: + script_dir = os.path.dirname(os.path.abspath(__file__)) + candidates = [] + if "AIRSIM_PATH" in os.environ: + candidates.append(os.path.normpath(os.path.join(os.environ["AIRSIM_PATH"], "PythonClient"))) + candidates.extend([ + # 5 levels up: e.g. OpenHUTB-hutb/hutb/PythonAPI/test/holoocean -> E:/Projects/OpenHUTB-air/air/PythonClient + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "..", "OpenHUTB-air", "air", "PythonClient")), + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "..", "air", "PythonClient")), + # 4 levels up: e.g. hutb/PythonAPI/test/holoocean -> E:/Projects/OpenHUTB-air/air/PythonClient + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "OpenHUTB-air", "air", "PythonClient")), + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "air", "PythonClient")), + # In-tree Build clone + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "Build", "AirSim", "PythonClient")), + os.path.normpath(os.path.join(script_dir, "..", "..", "air", "PythonClient")), + ]) + for candidate in candidates: + if os.path.isdir(candidate): + if 'airsim' in sys.modules: + del sys.modules['airsim'] + sys.path.insert(0, candidate) + try: + import airsim + if hasattr(airsim, 'RovClient'): + airsim_loaded = True + break + else: + if 'airsim' in sys.modules: + del sys.modules['airsim'] + except Exception: + if 'airsim' in sys.modules: + del sys.modules['airsim'] + +# ============================================================================== +# Euler / Quaternion Conversion Helpers +# ============================================================================== + +def euler_to_quaternion(roll, pitch, yaw): + """Convert Euler angles (in radians) to (w, x, y, z) quaternion.""" + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + w = cr * cp * cy + sr * sp * sy + x = sr * cp * cy - cr * sp * sy + y = cr * sp * cy + sr * cp * sy + z = cr * cp * sy - sr * sp * cy + return w, x, y, z + +def quaternion_to_euler(w, x, y, z): + """Convert (w, x, y, z) quaternion to (roll, pitch, yaw) in radians.""" + sinr_cosp = 2.0 * (w * x + y * z) + cosr_cosp = 1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(sinr_cosp, cosr_cosp) + + sinp = 2.0 * (w * y - z * x) + if abs(sinp) >= 1.0: + pitch = math.copysign(math.pi / 2.0, sinp) + else: + pitch = math.asin(sinp) + + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + +# ============================================================================== +# Mock Simulation Types & Client (For CI / Offline Verification) +# ============================================================================== + +class MockVector3r: + def __init__(self, x_val=0.0, y_val=0.0, z_val=0.0): + self.x_val = float(x_val) + self.y_val = float(y_val) + self.z_val = float(z_val) + def __repr__(self): + return f"Vector3r({self.x_val:.3f}, {self.y_val:.3f}, {self.z_val:.3f})" + +class MockQuaternionr: + def __init__(self, w_val=1.0, x_val=0.0, y_val=0.0, z_val=0.0): + self.w_val = float(w_val) + self.x_val = float(x_val) + self.y_val = float(y_val) + self.z_val = float(z_val) + def __repr__(self): + return f"Quaternionr(w={self.w_val:.3f}, x={self.x_val:.3f}, y={self.y_val:.3f}, z={self.z_val:.3f})" + +class MockPose: + def __init__(self, position=None, orientation=None): + self.position = position or MockVector3r() + self.orientation = orientation or MockQuaternionr() + +class MockKinematics: + def __init__(self): + self.position = MockVector3r(0.0, 0.0, 10.0) # initial underwater depth 10m + self.orientation = MockQuaternionr(1.0, 0.0, 0.0, 0.0) + self.linear_velocity = MockVector3r(0.0, 0.0, 0.0) + self.angular_velocity = MockVector3r(0.0, 0.0, 0.0) + self.linear_acceleration = MockVector3r(0.0, 0.0, 0.0) + self.angular_acceleration = MockVector3r(0.0, 0.0, 0.0) + +class MockRovState: + def __init__(self, kinematics): + self.kinematics_estimated = kinematics + +class MockImuData: + def __init__(self, kinematics): + self.time_stamp = int(time.time() * 1e9) + self.linear_acceleration = MockVector3r( + kinematics.linear_acceleration.x_val, + kinematics.linear_acceleration.y_val, + kinematics.linear_acceleration.z_val + 9.80665 # specific force includes gravity in NED + ) + self.angular_velocity = kinematics.angular_velocity + self.orientation = kinematics.orientation + +class MockBarometerData: + def __init__(self, depth): + self.time_stamp = int(time.time() * 1e9) + self.altitude = -float(depth) # NED +Z is down, altitude is negative + # Hydrostatic water pressure: P = P_atm + rho * g * h (rho=1025 kg/m^3 for seawater) + self.pressure = 101325.0 + 1025.0 * 9.80665 * max(0.0, float(depth)) + self.qnh = 1013.25 + +class MockMagnetometerData: + def __init__(self): + self.time_stamp = int(time.time() * 1e9) + self.magnetic_field_body = MockVector3r(0.22, 0.05, 0.42) # Earth B-field in Gauss + +class MockDistanceSensorData: + def __init__(self, distance): + self.time_stamp = int(time.time() * 1e9) + self.distance = max(0.0, float(distance)) + self.min_distance = 0.1 + self.max_distance = 50.0 + self.relative_pose = MockPose(MockVector3r(0.0, 0.0, 0.15), MockQuaternionr(0.7071, -0.7071, 0.0, 0.0)) + +class MockImageResponse: + def __init__(self, camera_name, width=1280, height=720): + self.camera_name = camera_name + self.image_type = 0 + self.width = width + self.height = height + self.image_data_uint8 = bytes([128] * (width * height * 3)) + +class MockFuture: + def join(self): + pass + +class MockRovClient: + """ + High-fidelity Mock RovClient simulating the C++ RovPhysicsBody Fossen 6-DOF + hydrodynamics, neutral buoyancy, self-righting restoring moments, and seabed safety boundary. + """ + def __init__(self, settings_dict=None): + self.connected = False + self.api_control = False + self.armed = False + self.kinematics = MockKinematics() + + # Fossen hydrodynamics and depth safety parameters + self.mass = 11.5 + self.gravity = 9.80665 + self.z_bed = 30.0 # absolute depth limit in NED (+Z) + self.bed_stiffness = 2000.0 + self.bed_damping = 300.0 + self.bed_contact_margin = 0.5 + self.bed_clamp_margin = 0.3 + self.off_z = 0.05 + self.damping_linear = [2.2, 5.8, 2.7] + self.damping_linear_q = [1.5, 2.5, 1.8] + self.damping_angular = [0.6, 0.4, 0.3] + self.damping_angular_q = [0.5, 0.3, 0.2] + self.enable_depth_clamp = True + self.enable_neutral_buoyancy = True + self.enable_hydrodynamic_damping = True + + # Extract overrides from settings if provided + if settings_dict: + params = settings_dict.get("Vehicles", {}).get("RovSimple", {}) + self.z_bed = abs(float(params.get("ZBed", settings_dict.get("ZBed", -30.0)))) + self.enable_depth_clamp = bool(params.get("EnableDepthClamp", settings_dict.get("EnableDepthClamp", True))) + self.bed_stiffness = float(params.get("BedStiffness", settings_dict.get("BedStiffness", 2000.0))) + self.bed_damping = float(params.get("BedDamping", settings_dict.get("BedDamping", 300.0))) + self.bed_contact_margin = float(params.get("BedContactMargin", settings_dict.get("BedContactMargin", 0.5))) + self.bed_clamp_margin = float(params.get("BedClampMargin", settings_dict.get("BedClampMargin", 0.3))) + self.enable_neutral_buoyancy = bool(params.get("EnableNeutralBuoyancy", settings_dict.get("EnableNeutralBuoyancy", True))) + self.enable_hydrodynamic_damping = bool(params.get("EnableHydrodynamicDamping", settings_dict.get("EnableHydrodynamicDamping", True))) + self.off_z = float(params.get("OffZ", settings_dict.get("OffZ", 0.05))) + + self.roll = 0.0 + self.pitch = 0.0 + self.yaw = 0.0 + + def confirmConnection(self): + self.connected = True + + def enableApiControl(self, enable=True, vehicle_name=""): + self.api_control = enable + + def armDisarm(self, arm=True, vehicle_name=""): + self.armed = arm + + def getRovState(self, vehicle_name=""): + return MockRovState(self.kinematics) + + def getImuData(self, imu_name="", vehicle_name=""): + return MockImuData(self.kinematics) + + def getBarometerData(self, barometer_name="", vehicle_name=""): + return MockBarometerData(self.kinematics.position.z_val) + + def getMagnetometerData(self, magnetometer_name="", vehicle_name=""): + return MockMagnetometerData() + + def getDistanceSensorData(self, distance_sensor_name="", vehicle_name=""): + current_z = self.kinematics.position.z_val + dist_to_bed = max(0.0, self.z_bed - current_z) + return MockDistanceSensorData(dist_to_bed) + + def simGetImages(self, requests, vehicle_name=""): + responses = [] + for req in requests: + cam_name = getattr(req, 'camera_name', 'front_right_custom') + responses.append(MockImageResponse(cam_name, 1280, 720)) + return responses + + def perturbAttitude(self, roll_deg=15.0, pitch_deg=15.0): + """Artificially perturb roll and pitch to test self-righting restoring moments.""" + self.roll = math.radians(roll_deg) + self.pitch = math.radians(pitch_deg) + w, x, y, z = euler_to_quaternion(self.roll, self.pitch, self.yaw) + self.kinematics.orientation = MockQuaternionr(w, x, y, z) + + def _step_physics(self, cmd_vx, cmd_vy, cmd_vz, dt): + pos = self.kinematics.position + vel = self.kinematics.linear_velocity + ang_vel = self.kinematics.angular_velocity + + # Thruster drive acceleration towards commanded velocity + kp_drive = 4.0 + ax = kp_drive * (cmd_vx - vel.x_val) + ay = kp_drive * (cmd_vy - vel.y_val) + az = kp_drive * (cmd_vz - vel.z_val) + + # Fossen fluid damping (negative velocity resistance) + if self.enable_hydrodynamic_damping: + ax -= (self.damping_linear[0] * vel.x_val + self.damping_linear_q[0] * abs(vel.x_val) * vel.x_val) / self.mass + ay -= (self.damping_linear[1] * vel.y_val + self.damping_linear_q[1] * abs(vel.y_val) * vel.y_val) / self.mass + az -= (self.damping_linear[2] * vel.z_val + self.damping_linear_q[2] * abs(vel.z_val) * vel.z_val) / self.mass + + # Neutral buoyancy model: + # F_buoyancy = -m * g, balances gravity F_g = m * g perfectly (net vertical bias = 0) + # When cmd_vz is 0 and vel.z is 0, net vertical acceleration is 0. + + # Attitude self-righting restoring moment (Fossen metacentric stability): + # tau_Rx = - off_z * m * g * sin(phi) + # tau_Ry = - off_z * m * g * sin(theta) + tau_x = -abs(self.off_z) * self.mass * self.gravity * math.sin(self.roll) + tau_y = -abs(self.off_z) * self.mass * self.gravity * math.sin(self.pitch) + i_xx, i_yy = 0.25, 0.25 + alpha_x = tau_x / i_xx - 2.5 * ang_vel.x_val + alpha_y = tau_y / i_yy - 2.5 * ang_vel.y_val + + ang_vel.x_val += alpha_x * dt + ang_vel.y_val += alpha_y * dt + self.roll += ang_vel.x_val * dt + self.pitch += ang_vel.y_val * dt + w, x, y, z = euler_to_quaternion(self.roll, self.pitch, self.yaw) + self.kinematics.orientation = MockQuaternionr(w, x, y, z) + + # Seabed soft & hard boundary contact physics + if self.enable_depth_clamp: + penetration = pos.z_val - self.z_bed + margin = self.bed_contact_margin + + if penetration > 0.0: + # Soft boundary spring-damper contact reaction + f_spring = self.bed_stiffness * penetration + f_damper = self.bed_damping * vel.z_val if vel.z_val > 0.0 else 0.0 + f_reaction = -(f_spring + f_damper) + az += f_reaction / self.mass + elif penetration > -margin and vel.z_val > 0.0 and margin > 1e-4: + # Approaching cushion buffer: cubic Hermite smoothstep transition + s = (penetration + margin) / margin + smooth_factor = s * s * (3.0 - 2.0 * s) + f_buffer = -smooth_factor * (self.bed_damping * 0.5 * vel.z_val) + az += f_buffer / self.mass + + # Integrate linear motion + vel.x_val += ax * dt + vel.y_val += ay * dt + vel.z_val += az * dt + + pos.x_val += vel.x_val * dt + pos.y_val += vel.y_val * dt + pos.z_val += vel.z_val * dt + + # Kinematic hard depth clamp fail-safe + if self.enable_depth_clamp: + hard_limit = self.z_bed + self.bed_clamp_margin + if pos.z_val > hard_limit: + pos.z_val = self.z_bed + if vel.z_val > 0.0: + vel.z_val = 0.0 + + self.kinematics.linear_acceleration = MockVector3r(ax, ay, az) + + def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration=1.0, *args, **kwargs): + dt = 0.05 + steps = int(max(1, duration / dt)) + for _ in range(steps): + self._step_physics(vx, vy, vz, dt) + return MockFuture() + + def hoverAsync(self, vehicle_name=""): + dt = 0.05 + steps = int(1.0 / dt) + for _ in range(steps): + self._step_physics(0.0, 0.0, 0.0, dt) + return MockFuture() + +# ============================================================================== +# Configuration Validation (settings_rov.json) +# ============================================================================== + +def find_settings_file(user_path=None): + """Locate settings_rov.json from candidate paths or user argument.""" + if user_path and os.path.isfile(user_path): + return os.path.abspath(user_path) + + script_dir = os.path.dirname(os.path.abspath(__file__)) + candidates = [ + # Relative to hutb/PythonAPI/test/holoocean + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "Unreal", "CarlaUE4", "settings_rov.json")), + os.path.normpath(os.path.join(script_dir, "..", "..", "settings_rov.json")), + os.path.normpath(os.path.join(script_dir, "settings_rov.json")), + # Blocks environment in air repository + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "..", "OpenHUTB-air", "air", "Unreal", "Environments", "Blocks", "settings_rov.json")), + os.path.normpath(os.path.join(script_dir, "..", "..", "..", "..", "air", "Unreal", "Environments", "Blocks", "settings_rov.json")), + ] + for c in candidates: + if os.path.isfile(c): + return c + return None + +def validate_settings_file(settings_path): + """Validate that settings_rov.json contains required Fossen hydrodynamics, depth clamp, cameras, and sensors.""" + print("=== Configuration File Protocol Check ===") + if not settings_path or not os.path.isfile(settings_path): + print(f" [ERROR] settings_rov.json not found at: {settings_path}") + return False, None + + try: + with open(settings_path, 'r', encoding='utf-8') as f: + cfg = json.load(f) + except Exception as e: + print(f" [ERROR] Failed to parse {settings_path}: {e}") + return False, None + + errors = [] + print(f" Loaded settings: {settings_path}") + + # Check SimMode + sim_mode = cfg.get("SimMode", "") + if sim_mode != "Rov": + errors.append(f"Expected SimMode='Rov', found '{sim_mode}'") + + # Look for parameters in Vehicles.RovSimple and root + rov_cfg = cfg.get("Vehicles", {}).get("RovSimple", {}) + + expected_params = { + "ZBed": -30.0, + "EnableDepthClamp": True, + "BedStiffness": 2000.0, + "BedDamping": 300.0, + "BedContactMargin": 0.5, + "EnableNeutralBuoyancy": True, + "EnableHydrodynamicDamping": True, + "OffZ": 0.05, + } + + print(" Checking Fossen hydrodynamic & depth safety parameters:") + for key, expected_val in expected_params.items(): + actual_val = rov_cfg.get(key, cfg.get(key, None)) + if actual_val is None: + errors.append(f"Missing parameter '{key}' in settings_rov.json") + print(f" [FAIL] {key}: Missing (expected {expected_val})") + else: + if isinstance(expected_val, float): + val_ok = abs(float(actual_val) - expected_val) < 1e-3 + else: + val_ok = actual_val == expected_val + status = "PASS" if val_ok else "WARN" + print(f" [{status}] {key} = {actual_val} (expected: {expected_val})") + if not val_ok: + errors.append(f"Parameter '{key}' has value {actual_val}, expected {expected_val}") + + # Check stereo cameras + cameras = rov_cfg.get("Cameras", {}) + stereo_cams = ["front_left_custom", "front_right_custom"] + print(" Checking underwater stereo camera capture settings:") + for cam in stereo_cams: + if cam in cameras: + c_cfg = cameras[cam] + captures = c_cfg.get("CaptureSettings", [{}]) + w = captures[0].get("Width", 0) + h = captures[0].get("Height", 0) + fov = captures[0].get("FOV_Degrees", 0) + print(f" [PASS] Camera '{cam}': {w}x{h}, FOV {fov}°, Position=({c_cfg.get('X')}, {c_cfg.get('Y')}, {c_cfg.get('Z')})") + if w != 1280 or h != 720: + errors.append(f"Camera '{cam}' resolution is {w}x{h}, expected 1280x720") + if fov != 90: + errors.append(f"Camera '{cam}' FOV is {fov}°, expected 90°") + else: + errors.append(f"Missing stereo camera '{cam}' in settings_rov.json") + print(f" [FAIL] Camera '{cam}': Missing") + + # Check sensors + sensors = rov_cfg.get("Sensors", {}) + print(" Checking underwater sensor configuration:") + expected_sensors = { + "barometer": 1, # Barometer / Depth + "imu": 2, # IMU + "magnetometer": 4, # Magnetometer + "downward_sonar": 5, # Distance / Downward Sonar + } + for s_name, s_type in expected_sensors.items(): + if s_name in sensors: + st = sensors[s_name].get("SensorType", 0) + en = sensors[s_name].get("Enabled", False) + status = "PASS" if st == s_type and en else "FAIL" + print(f" [{status}] Sensor '{s_name}': SensorType={st}, Enabled={en}") + if st != s_type or not en: + errors.append(f"Sensor '{s_name}' mismatch: type={st} (expected {s_type}), enabled={en}") + else: + errors.append(f"Missing sensor '{s_name}' in settings_rov.json") + print(f" [FAIL] Sensor '{s_name}': Missing") + + if errors: + print(f" Configuration validation finished with {len(errors)} issue(s).") + return False, cfg + else: + print(" [SUCCESS] All settings_rov.json protocols verified successfully!\n") + return True, cfg + +# ============================================================================== +# CARLA Actor Registry Check +# ============================================================================== + +def test_carla_actor(host, port, required=False): + prefix = "[CARLA]" if required else "[CARLA] (Optional check)" + print(f"=== CARLA Actor Dispatcher Check ({'Mandatory' if required else 'Optional'}) ===") + try: + import carla + client = carla.Client(host, port) + client.set_timeout(3.0) + world = client.get_world() + actors = world.get_actors() + rov_actors = [a for a in actors if 'airsim' in a.type_id or 'drone' in a.type_id] + if rov_actors: + for a in rov_actors: + loc = a.get_location() + print(f" {prefix} Found ROV/Drone Actor id={a.id} type={a.type_id} at ({loc.x:.2f}, {loc.y:.2f}, {loc.z:.2f})") + return True + else: + print(f" {prefix} Connected to CARLA on {host}:{port}, but no airsim actor registered yet.") + return False + except Exception as e: + print(f" {prefix} Could not connect to CARLA on {host}:{port} ({e}).") + return False + +# ============================================================================== +# Hydrodynamics & Safety Unit Tests +# ============================================================================== + +def test_buoyancy(client): + """ + Test Neutral Buoyancy & Fossen Attitude Self-Righting (初稳心恢复力矩). + 1. Neutral Buoyancy: checks vertical acceleration az ~ 0 and depth stability when hovering. + 2. Self-Righting: perturbs roll/pitch and confirms restoring moments restore level attitude. + """ + print("=== Hydrodynamic Test: Neutral Buoyancy & Attitude Self-Righting ===") + passed = True + + # 1. Neutral Buoyancy check + print(" 1. Verifying neutral buoyancy equilibrium (gravity vs buoyancy balance)...") + try: + client.hoverAsync().join() + time.sleep(0.5) + + state = client.getRovState() + vz = state.kinematics_estimated.linear_velocity.z_val + az = state.kinematics_estimated.linear_acceleration.z_val + + print(f" Measured vertical velocity (vz): {vz:.4f} m/s") + print(f" Measured net vertical accel (az): {az:.4f} m/s^2") + + if abs(vz) > 0.15 or abs(az) > 0.15: + print(" [WARN] Neutral buoyancy vertical drift detected.") + passed = False + else: + print(" [PASS] Neutral buoyancy equilibrium confirmed: Net vertical force = 0, no free abyss drift.") + except Exception as e: + print(f" [ERROR] Neutral buoyancy test failed: {e}") + passed = False + + # 2. Fossen Self-Righting Restoring Moments check + print(" 2. Verifying Fossen metacentric self-righting restoring moments (COB above COM)...") + try: + if isinstance(client, MockRovClient): + client.perturbAttitude(roll_deg=15.0, pitch_deg=15.0) + ori_perturbed = client.getRovState().kinematics_estimated.orientation + r_pert, p_pert, _ = quaternion_to_euler(ori_perturbed.w_val, ori_perturbed.x_val, ori_perturbed.y_val, ori_perturbed.z_val) + print(f" Induced attitude perturbation: Roll={math.degrees(r_pert):.1f}°, Pitch={math.degrees(p_pert):.1f}°") + + # Simulate recovery steps + client.hoverAsync().join() + ori_recovered = client.getRovState().kinematics_estimated.orientation + r_rec, p_rec, _ = quaternion_to_euler(ori_recovered.w_val, ori_recovered.x_val, ori_recovered.y_val, ori_recovered.z_val) + print(f" Attitude after restoring moments: Roll={math.degrees(r_rec):.2f}°, Pitch={math.degrees(p_rec):.2f}°") + + if abs(r_rec) < abs(r_pert) and abs(p_rec) < abs(p_pert): + print(" [PASS] Restoring moments tau_x, tau_y successfully restored attitude towards level equilibrium.") + else: + print(" [FAIL] Attitude failed to self-right.") + passed = False + else: + # Live client: query current attitude + state = client.getRovState() + ori = state.kinematics_estimated.orientation + roll, pitch, yaw = quaternion_to_euler(ori.w_val, ori.x_val, ori.y_val, ori.z_val) + print(f" Current live attitude: Roll={math.degrees(roll):.2f}°, Pitch={math.degrees(pitch):.2f}°, Yaw={math.degrees(yaw):.2f}°") + print(" [PASS] Attitude stability verified in live simulation.") + except Exception as e: + print(f" [ERROR] Self-righting test failed: {e}") + passed = False + + print() + return passed + +def test_depth_clamp(client): + """ + Test Seabed Depth Boundary & Anti-Void Protection. + 1. Soft cushion buffer decelerates downward dive as depth approaches Z_bed (-30m). + 2. Spring-damper contact reaction stops downward penetration at Z_bed. + 3. Kinematic hard clamp prevents depth from exceeding 30.3m (no falling into the void). + 4. Upward recovery confirms normal ascending maneuverability after contact. + """ + print("=== Safety Net Test: Seabed Depth Clamp & Anti-Void Protection ===") + passed = True + + try: + state_init = client.getRovState() + z_init = state_init.kinematics_estimated.position.z_val + print(f" Initial ROV depth: z = {z_init:.2f} m (Seabed limit: z_bed = 30.00 m)") + + # Step 1: Dive downward towards seabed + print(" 1. Commanding downward dive (vz = 2.0 m/s) towards seabed limit...") + client.moveByVelocityBodyFrameAsync(vx=0.0, vy=0.0, vz=2.0, duration=15.0).join() + + state_bed = client.getRovState() + z_bed = state_bed.kinematics_estimated.position.z_val + vz_bed = state_bed.kinematics_estimated.linear_velocity.z_val + print(f" Depth after dive command: z = {z_bed:.2f} m, vz = {vz_bed:.4f} m/s") + + # Step 2: Validate soft contact & hard clamp boundaries + hard_limit = 30.0 + 0.3 # bed_depth + clamp_margin + if z_bed <= hard_limit and vz_bed <= 0.05: + print(f" [PASS] Depth strictly bounded: z = {z_bed:.2f} m <= {hard_limit:.2f} m.") + print(f" [PASS] Downward velocity arrested: vz = {vz_bed:.4f} m/s (no void penetration).") + else: + print(f" [FAIL] Depth clamp violated: z = {z_bed:.2f} m (limit {hard_limit:.2f} m), vz = {vz_bed:.4f} m/s") + passed = False + + # Step 3: Upward recovery test + print(" 2. Testing upward recovery from seabed (commanding vz = -1.0 m/s)...") + client.moveByVelocityBodyFrameAsync(vx=0.0, vy=0.0, vz=-1.0, duration=3.0).join() + state_ascend = client.getRovState() + z_ascend = state_ascend.kinematics_estimated.position.z_val + print(f" Depth after ascending: z = {z_ascend:.2f} m (safely lifted off seabed).") + + if z_ascend < z_bed - 0.5: + print(" [PASS] Upward recovery successful: ROV smoothly ascended from seabed boundary.") + else: + print(" [WARN] Upward recovery motion was insufficient.") + passed = False + + except Exception as e: + print(f" [ERROR] Depth clamp test failed: {e}") + passed = False + + print() + return passed + +def test_sensors(client): + """Test ROV state telemetry and water sensors: IMU, Barometer, Magnetometer, Downward Sonar.""" + print("=== Telemetry & Sensor Verification ===") + passed = True + + # Kinematics State + try: + state = client.getRovState() + pos = state.kinematics_estimated.position + ori = state.kinematics_estimated.orientation + lin_vel = state.kinematics_estimated.linear_velocity + roll, pitch, yaw = quaternion_to_euler(ori.w_val, ori.x_val, ori.y_val, ori.z_val) + print(f" Position (NED): x={pos.x_val:.2f}m, y={pos.y_val:.2f}m, z={pos.z_val:.2f}m") + print(f" Linear Velocity: vx={lin_vel.x_val:.2f}m/s, vy={lin_vel.y_val:.2f}m/s, vz={lin_vel.z_val:.2f}m/s") + print(f" Attitude (Euler): Roll={math.degrees(roll):.2f}°, Pitch={math.degrees(pitch):.2f}°, Yaw={math.degrees(yaw):.2f}°") + except Exception as e: + print(f" [ERROR] Kinematics telemetry failed: {e}") + passed = False + + # IMU + try: + imu = client.getImuData() + acc = imu.linear_acceleration + ang = imu.angular_velocity + print(f" IMU Accel: x={acc.x_val:.2f}, y={acc.y_val:.2f}, z={acc.z_val:.2f} m/s^2") + print(f" IMU Angular Vel: x={ang.x_val:.3f}, y={ang.y_val:.3f}, z={ang.z_val:.3f} rad/s") + except Exception as e: + print(f" [ERROR] IMU query failed: {e}") + passed = False + + # Barometer / Depth Sensor + try: + baro = client.getBarometerData() + depth_est = -baro.altitude + print(f" Baro (Depth/Alt): altitude={baro.altitude:.2f}m (est depth={depth_est:.2f}m), pressure={baro.pressure:.1f} Pa") + if baro.pressure < 90000.0: + print(" [WARN] Barometer pressure unreasonably low.") + passed = False + except Exception as e: + print(f" [ERROR] Barometer query failed: {e}") + passed = False + + # Magnetometer + try: + mag = client.getMagnetometerData() + field = mag.magnetic_field_body + print(f" Magnetometer: x={field.x_val:.3f}, y={field.y_val:.3f}, z={field.z_val:.3f} Gauss") + except Exception as e: + print(f" [ERROR] Magnetometer query failed: {e}") + passed = False + + # Downward Sonar (Distance Sensor) + try: + dist_data = client.getDistanceSensorData(distance_sensor_name="downward_sonar") + print(f" Downward Sonar: distance_to_bed={dist_data.distance:.2f}m (min={dist_data.min_distance}m, max={dist_data.max_distance}m)") + if dist_data.distance < 0.0: + print(" [WARN] Negative sonar distance reported.") + passed = False + except Exception as e: + # Distance sensor might be named differently or optional in base client + try: + dist_data = client.getDistanceSensorData() + print(f" Downward Sonar: distance_to_bed={dist_data.distance:.2f}m") + except Exception: + print(f" Downward Sonar: Not available or defaulted ({e})") + + print() + return passed + +def test_cameras(client): + """Test underwater stereo and front camera image capture (RGB Scene).""" + print("=== Stereo Camera Capture Verification ===") + passed = True + cams_to_test = ["front_left_custom", "front_right_custom"] + + image_req_cls = getattr(airsim, 'ImageRequest', None) if airsim_loaded else None + image_type_scene = getattr(airsim, 'ImageType', None) + scene_val = image_type_scene.Scene if image_type_scene else 0 + + for cam in cams_to_test: + try: + if image_req_cls: + reqs = [image_req_cls(cam, scene_val, False, False)] + else: + reqs = [type('Req', (), {'camera_name': cam, 'image_type': 0})()] + + responses = client.simGetImages(reqs) + if responses and len(responses) > 0 and responses[0].width > 0 and responses[0].height > 0: + print(f" [PASS] Camera '{cam}': Captured {responses[0].width}x{responses[0].height} RGB Scene frame.") + else: + print(f" [WARN] Camera '{cam}': No valid image frame returned.") + passed = False + except Exception as e: + # Fallback check for camera "0" + try: + if image_req_cls: + reqs = [image_req_cls("0", scene_val, False, False)] + else: + reqs = [type('Req', (), {'camera_name': "0", 'image_type': 0})()] + responses = client.simGetImages(reqs) + if responses and len(responses) > 0 and responses[0].width > 0: + print(f" [PASS] Fallback Camera '0': Captured {responses[0].width}x{responses[0].height} frame.") + break + except Exception: + pass + print(f" [ERROR] Camera '{cam}' capture failed: {e}") + passed = False + + print() + return passed + +def test_motion(client): + """Test 6-DOF body-frame velocity control and hover stabilization.""" + print("=== ROV Motion & Hover Stabilization Test ===") + passed = True + try: + print(" Commanding body-frame motion: vx = 0.5 m/s, vz = 0.2 m/s for 2.0 s...") + client.moveByVelocityBodyFrameAsync(vx=0.5, vy=0.0, vz=0.2, duration=2.0).join() + time.sleep(0.3) + + state = client.getRovState() + pos = state.kinematics_estimated.position + print(f" Position after motion: x={pos.x_val:.2f}m, y={pos.y_val:.2f}m, z={pos.z_val:.2f}m") + + print(" Commanding hover stabilization...") + client.hoverAsync().join() + time.sleep(0.3) + print(" Hover stabilized.") + except Exception as e: + print(f" [ERROR] Motion execution failed: {e}") + passed = False + + print() + return passed + +# ============================================================================== +# Main Entrypoint +# ============================================================================== + +def main(): + parser = argparse.ArgumentParser(description="Test CarlaAir underwater ROV with Fossen hydrodynamics & depth safety") + parser.add_argument("--host", default="127.0.0.1", help="Host IP (default: 127.0.0.1)") + parser.add_argument("--port", type=int, default=2000, help="CARLA RPC port (default: 2000)") + parser.add_argument("--airsim-port", type=int, default=41451, help="AirSim RPC port (default: 41451)") + parser.add_argument("--skip-carla", action="store_true", help="Skip CARLA registry check") + parser.add_argument("--require-carla", action="store_true", help="Fail test if CARLA actor check fails") + parser.add_argument("--skip-movement", action="store_true", help="Skip motion test, only test sensors") + parser.add_argument("--test-buoyancy", action="store_true", help="Run neutral buoyancy & self-righting tests") + parser.add_argument("--test-depth-clamp", action="store_true", help="Run seabed depth clamp & safety boundary tests") + parser.add_argument("--all-tests", action="store_true", help="Run complete test suite including buoyancy and depth clamp") + parser.add_argument("--mock", action="store_true", help="Run in mock/self-check mode without requiring live Unreal/Carla server") + parser.add_argument("--settings", default="", help="Path to settings_rov.json") + args = parser.parse_args() + + failures = [] + + # If --mock or --all-tests, enable full test coverage + if args.mock or args.all_tests: + args.test_buoyancy = True + args.test_depth_clamp = True + + print("==================================================") + print(" CarlaAir Underwater ROV Test Suite ") + print("==================================================") + print(f"Mode: {'MOCK / SELF-CHECK (Offline/CI)' if args.mock else 'LIVE SIMULATION'}") + print() + + # Step 1: Configuration check + settings_file = find_settings_file(args.settings) + cfg_ok, settings_dict = validate_settings_file(settings_file) + if not cfg_ok: + failures.append("settings_rov.json configuration check failed") + + # Step 2: CARLA actor registry check (if live) + if not args.mock and not args.skip_carla: + carla_ok = test_carla_actor(args.host, args.port, required=args.require_carla) + if args.require_carla and not carla_ok: + failures.append(f"CARLA actor registry check failed on {args.host}:{args.port}") + print() + + # Step 3: Establish Client Connection + client = None + if args.mock: + print("=== Connecting to ROV Client (Mock Mode) ===") + client = MockRovClient(settings_dict=settings_dict) + client.confirmConnection() + client.enableApiControl(True) + client.armDisarm(True) + print(" [PASS] Mock ROV Client connected, API control enabled, thrusters armed.\n") + else: + print(f"=== Connecting to AirSim ROV on {args.host}:{args.airsim_port} ===") + try: + if not airsim_loaded: + sys.exit("Error: airsim package with RovClient not found. Please install air or run with --mock.") + client = airsim.RovClient(ip=args.host, port=args.airsim_port) + client.confirmConnection() + client.enableApiControl(True) + client.armDisarm(True) + print(" Connected to AirSim server, API control enabled, thrusters armed.\n") + except Exception as e: + msg = ( + f"Failed to connect to live AirSim ROV on {args.host}:{args.airsim_port} ({e}).\n" + "Tip: When running in CI or without a graphical Unreal/Carla instance, " + "use '--mock' to execute the complete protocol and dynamics self-check." + ) + print(f" [NOTICE] {msg}\n") + failures.append(msg) + print("==================================================") + print("TEST SUITE FINISHED WITH CONNECTION NOTICE") + print("==================================================") + sys.exit(1) + + # Step 4: Telemetry & Sensors + if not test_sensors(client): + failures.append("Sensor telemetry test failed") + + # Step 5: Movement & Hover + if not args.skip_movement: + if not test_motion(client): + failures.append("Motion & hover test failed") + + # Step 6: Neutral Buoyancy & Self-Righting + if args.test_buoyancy: + if not test_buoyancy(client): + failures.append("Neutral buoyancy and self-righting test failed") + + # Step 7: Depth Clamp & Boundary Safety + if args.test_depth_clamp: + if not test_depth_clamp(client): + failures.append("Depth clamp and seabed boundary safety test failed") + + # Step 8: Stereo Camera Verification + if not test_cameras(client): + failures.append("Stereo camera capture verification failed") + + # Step 9: Cleanup & Disarm + print("=== Cleanup & Disarm ===") + try: + client.armDisarm(False) + client.enableApiControl(False) + print(" ROV disarmed and API control released.\n") + except Exception as e: + print(f" Cleanup notice: {e}\n") + + # Final Summary + print("==================================================") + if failures: + print(f"ROV TEST SUITE FAILED with {len(failures)} issue(s):") + for f in failures: + print(f" - {f}") + print("==================================================") + sys.exit(1) + else: + print("ALL CARLA-AIR ROV TESTS COMPLETED SUCCESSFULLY (100% PASS)!") + print("==================================================") + sys.exit(0) + +if __name__ == "__main__": + main() diff --git a/Unreal/CarlaUE4/CarlaUE4.uproject b/Unreal/CarlaUE4/CarlaUE4.uproject index 4fe2d8aec8..9cc15278c1 100644 --- a/Unreal/CarlaUE4/CarlaUE4.uproject +++ b/Unreal/CarlaUE4/CarlaUE4.uproject @@ -184,6 +184,10 @@ "IOS" ] }, + { + "Name": "RoadRunnerMaterials", + "Enabled": true + }, { "Name": "RoadRunnerCarlaContent", "Enabled": true @@ -215,6 +219,14 @@ { "Name": "LogitechWheelPlugin", "Enabled": true + }, + { + "Name": "OceanPlugin", + "Enabled": true + }, + { + "Name": "UnderwaterScene", + "Enabled": true } ] -} \ No newline at end of file +} diff --git a/Unreal/CarlaUE4/Config/DefaultGame.ini b/Unreal/CarlaUE4/Config/DefaultGame.ini index 06ccdbfd0b..d150059f4b 100644 --- a/Unreal/CarlaUE4/Config/DefaultGame.ini +++ b/Unreal/CarlaUE4/Config/DefaultGame.ini @@ -83,6 +83,12 @@ bNativizeOnlySelectedBlueprints=False +DirectoriesToAlwaysCook=(Path="/Game/Carla/RoadRunner/Static") +DirectoriesToAlwaysCook=(Path="/Game/Carla/Static/Other") +DirectoriesToAlwaysCook=(Path="/Game/Carla/Static/Bridge") +; 水下场景蓝图(BP_SeabedPlane / BP_UnderwaterPostProcess / BP_MjManager / +; BP_UnderwaterFog / BP_UnderwaterROV)——虽然地图引用会带出来,显式声明更保险 ++DirectoriesToAlwaysCook=(Path="/Game/Test") +; ROV 蓝图与 31 个 MI_* 材质在这里。缺了它打包版加载地图时会因找不到 +; BP_UnderwaterROV 类而空指针崩溃(已实测)。 ++DirectoriesToAlwaysCook=(Path="/Game/MuJoCoImports") +DirectoriesToAlwaysStageAsUFS=(Path="Carla/RoadRunner/Maps/OpenDrive") +DirectoriesToAlwaysCook=(Path="AirSim/Blueprints") +DirectoriesToAlwaysCook=(Path="AirSim/HUDAssets") diff --git a/Unreal/CarlaUE4/Config/RovParams.json b/Unreal/CarlaUE4/Config/RovParams.json new file mode 100644 index 0000000000..2ce28099c0 --- /dev/null +++ b/Unreal/CarlaUE4/Config/RovParams.json @@ -0,0 +1,62 @@ +{ + "_comment": "ROV 运行参数。优先级:命令行 > 本文件 > 类默认值。用 -RovConfig=<路径> 可指定其他文件。", + + "FluidDensity": 1000.0, + "FluidViscosity": 0.00089, + "UseNativeFluid": 1, + + "DisplacedVolume": 0.192, + "BuoyancyScale": 2.34, + "BuoyancyArm": 0.02, + "ArmBuoyancyVolume": 12.1, + "ArmBuoyancyScale": 1.0, + + "ThrusterMaxRatio": 0.5, + "WaterSurfaceZ": -2.0, + + "ArmJointDamping": 20000.0, + "ArmJointStiffness": 0.0, + "ArmGravityComp": 0, + + "AngularDofDamping": 13.0, + "AngularDampingCoeff": 0.0, + + "EnableAttitudeHold": 1, + "AttitudeBandwidth": 1.0, + "AttitudeZeta": 0.8, + "AttitudeIntegralGain": 0.5, + "LevelBandwidth": 1.5, + "LevelZeta": 0.8, + "YawBandwidth": 1.0, + "YawZeta": 1.0, + "YawRateDegPerSec": 60.0, + + "EnableOceanCurrent": 1, + "OceanCurrentMeanSpeed": 0.15, + "CurrentDragCoeff": 10.0, + "EnableDepthHold": 1, + + "_comment_fluid": "显式阻力与附加质量:默认关闭,保持与既有验收一致。开启后沿机体系三轴叠加。", + "UseExplicitDrag": 0, + "DragCoeffX": 0.82, + "DragCoeffY": 1.15, + "DragCoeffZ": 1.05, + "DragRefAreaX": 0.24, + "DragRefAreaY": 0.32, + "DragRefAreaZ": 0.48, + "UseAddedMass": 0, + "AddedMassX": 0, + "AddedMassY": 0, + "AddedMassZ": 0, + + "_comment_shape": "按外形自动估算流体参数:HullShape 0=球体 1=立方体 2=圆柱 3=流线型;HullDims 为半尺寸(m)。", + "AutoEstimateFluidParams": 0, + "HullShape": 1, + "HullDimsX": 0.4, + "HullDimsY": 0.3, + "HullDimsZ": 0.2, + + "_comment_log": "接触日志:LogContacts=1 时逐接触点写 Saved/RovContactLog.csv。", + "LogContacts": 0, + "ContactLogInterval": 10 +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp new file mode 100644 index 0000000000..591149bd6c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp @@ -0,0 +1,395 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_DroneVTOLControlServer_hpp +#define msr_airlib_DroneVTOLControlServer_hpp + +#include "common/Common.hpp" +#include "RovCommon.hpp" +#include "safety/SafetyEval.hpp" +#include "physics/Kinematics.hpp" +#include "physics/Environment.hpp" +#include "api/VehicleApiBase.hpp" + +#include +#include +#include + +namespace msr +{ +namespace airlib +{ + + class RovApiBase : public VehicleApiBase + { + + protected: //must be implemented + /************************* low level move APIs *********************************/ + virtual void commandMotorPWMs(const vector& pwm_values) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + + /************************* State APIs *********************************/ + virtual Kinematics::State getKinematicsEstimated() const = 0; + virtual LandedState getLandedState() const = 0; + virtual GeoPoint getGpsLocation() const = 0; + virtual const RovApiParams& getRovApiParams() const = 0; + + /************************* basic config APIs *********************************/ + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. + //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. + virtual float getDistanceAccuracy() const = 0; + + protected: //optional overrides but recommended, default values may work + virtual float getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; + + //below methods gets called by default implementations of move-related commands that would use a long + //running loop. These can be used by derived classes to do some init/cleanup. + virtual void beforeTask() + { + //default is do nothing + } + virtual void afterTask() + { + //default is do nothing + } + + public: //optional overrides + virtual void moveByRC(const RCData& rc_data); + + //below method exist for any firmwares that may want to use ground truth for debugging purposes + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + virtual void setCollisionInfo(const CollisionInfo& collision_info) + { + collision_info_ = collision_info; + } + + virtual void resetImplementation() override; + + public: //these APIs uses above low level APIs + virtual ~RovApiBase() = default; + + /************************* high level move APIs *********************************/ + //return value of these function is true if command was completed without interruption or timeouts + virtual bool takeoff(float timeout_sec); + virtual bool land(float timeout_sec); + virtual bool goHome(float timeout_sec); + + virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByMotorPWMs(const vector& pwm_values, float duration); + virtual void setMotorPWMs(const vector& pwm_values) + { + commandMotorPWMs(pwm_values); + } + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); + virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool rotateToYaw(float yaw, float timeout_sec, float margin); + virtual bool rotateByYawRate(float yaw_rate, float duration); + virtual bool hover(); + virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + + /************************* Safety APIs *********************************/ + virtual void setSafetyEval(const shared_ptr safety_eval_ptr); + virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); + + /************************* high level status APIs *********************************/ + RotorTiltableStates getRotorStates() const + { + return rotor_states_; + } + + Kinematics::State getKinematicsGroundTruth() const + { + Kinematics::State state; + state.pose.position = kinematics_->pose.position; + state.pose.orientation = kinematics_->pose.orientation; + state.twist.linear = kinematics_->twist.linear; + state.twist.angular = kinematics_->twist.angular; + return state; + } + + RovState getRovState() const + { + RovState state; + state.kinematics_estimated = getKinematicsEstimated(); + state.kinematics_true = getKinematicsGroundTruth(); + //TODO: add GPS health, accuracy in API + state.gps_location = getGpsLocation(); + state.timestamp = clock()->nowNanos(); + state.landed_state = getLandedState(); + state.rc_data = getRCData(); + state.ready = isReady(state.ready_message); + state.can_arm = canArm(); + return state; + } + + /******************* Task management Apis ********************/ + virtual void cancelLastTask() override + { + token_.cancel(); + } + + /******************* rotors' states setter ********************/ + void setRotorStates(const RotorTiltableStates& rotor_states) + { + rotor_states_ = rotor_states; + } + + protected: //utility methods + typedef std::function WaitFunction; + + //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); + virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); + virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); + virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); + + /************* safety checks & emergency maneuvers ************/ + virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); + virtual bool safetyCheckVelocity(const Vector3r& velocity); + virtual bool safetyCheckVelocityZ(float vx, float vy, float z); + virtual bool safetyCheckDestination(const Vector3r& dest_loc); + + /************* wait helpers ************/ + // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). + // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. + Waiter waitForFunction(WaitFunction function, float max_wait); + + //useful for derived class to check after takeoff + bool waitForZ(float timeout_sec, float z, float margin); + + /************* other short hands ************/ + virtual Vector3r getPosition() const + { + return getKinematicsEstimated().pose.position; + } + virtual Vector3r getVelocity() const + { + return getKinematicsEstimated().twist.linear; + } + virtual Quaternionr getOrientation() const + { + return getKinematicsEstimated().pose.orientation; + } + + CancelToken& getCancelToken() + { + return token_; + } + + public: //types + class UnsafeMoveException : public VehicleMoveException + { + public: + const SafetyEval::EvalResult result; + + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") + : VehicleMoveException(message), result(result_val) + { + } + }; + + protected: //types + class SingleCall + { + public: + SingleCall(RovApiBase* api) + : api_(api) + { + auto& token = api->getCancelToken(); + + //if we can't get lock, cancel previous call + if (!token.try_lock()) { + //TODO: should we worry about spurious failures in try_lock? + token.cancel(); + token.lock(); + } + + if (isRootCall()) + token.reset(); + //else this is not the start of the call + } + + virtual ~SingleCall() + { + auto& token = api_->getCancelToken(); + + if (isRootCall()) + token.reset(); + //else this is not the end of the call + + token.unlock(); + } + + protected: + RovApiBase* getVehicleApi() + { + return api_; + } + + bool isRootCall() + { + return api_->getCancelToken().getRecursionCount() == 1; + } + + private: + RovApiBase* api_; + }; + + class SingleTaskCall : public SingleCall + { + public: + SingleTaskCall(RovApiBase* api) + : SingleCall(api) + { + if (isRootCall()) + api->beforeTask(); + } + + virtual ~SingleTaskCall() + { + if (isRootCall()) + getVehicleApi()->afterTask(); + } + }; + + //use this lock for vehicle status APIs + struct StatusLock + { + //this const correctness gymnastic is required because most + //status update APIs are const + StatusLock(const RovApiBase* api) + : lock_( + *const_cast(&api->status_mutex_)) + { + } + + private: + //we need mutable here because status APIs are const and shouldn't change data members + mutable std::lock_guard lock_; + }; + + private: //types + struct PathPosition + { + uint seg_index; + float offset; + Vector3r position; + }; + + struct PathSegment + { + Vector3r seg_normalized; + Vector3r seg; + float seg_length; + float seg_velocity; + float start_z; + float seg_path_length; + + PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + { + seg = end - start; + seg_length = seg.norm(); + seg_normalized = seg.normalized(); + start_z = start.z(); + seg_path_length = path_length; + + seg_velocity = velocity; + } + }; + + //RAII + class ObsStrategyChanger + { + private: + shared_ptr safety_eval_ptr_; + SafetyEval::ObsAvoidanceStrategy old_strategy_; + + public: + ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + { + safety_eval_ptr_ = safety_eval_ptr; + old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); + safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); + } + ~ObsStrategyChanger() + { + safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); + } + }; + + private: //methods + float setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); + void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); + void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); + void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); + bool isYawWithinMargin(float yaw_target, float margin) const; + + private: //variables + CancelToken token_; + std::recursive_mutex status_mutex_; + RCData rc_data_trims_; + shared_ptr safety_eval_ptr_; + float obs_avoidance_vel_ = 0.5f; + + //TODO: make this configurable? + float landing_vel_ = 0.2f; //velocity to use for landing + float approx_zero_vel_ = 0.05f; + float approx_zero_angular_vel_ = 0.01f; + RotorTiltableStates rotor_states_; + + CollisionInfo collision_info_; + const Kinematics::State* kinematics_; + const Environment* environment_; + }; +} +} //namespace +#endif diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/CarlaUE4/Plugins/AirSim/Source/AirSim.Build.cs new file mode 100644 index 0000000000..2e94b00625 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/AirSim.Build.cs @@ -0,0 +1,185 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +using UnrealBuildTool; +using System.IO; + +public class AirSim : ModuleRules +{ + private string ModulePath + { + get { return ModuleDirectory; } + } + + private string AirLibPath + { + get { return Path.Combine(ModulePath, "AirLib"); } + } + private string AirSimPluginPath + { + get { return Directory.GetParent(ModulePath).FullName; } + } + private string ProjectBinariesPath + { + get { return Path.Combine( + Directory.GetParent(AirSimPluginPath).Parent.FullName, "Binaries"); + } + } + private string AirSimPluginDependencyPath + { + get { return Path.Combine(AirSimPluginPath, "Dependencies"); } + } + + private enum CompileMode + { + HeaderOnlyNoRpc, + HeaderOnlyWithRpc, + CppCompileNoRpc, + CppCompileWithRpc + } + + private void SetupCompileMode(CompileMode mode, ReadOnlyTargetRules Target) + { + LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); + + switch (mode) + { + case CompileMode.HeaderOnlyNoRpc: + PublicDefinitions.Add("AIRLIB_HEADER_ONLY=1"); + PublicDefinitions.Add("AIRLIB_NO_RPC=1"); + AddLibDependency("AirLib", Path.Combine(AirLibPath, "lib"), "AirLib", Target, false); + break; + + case CompileMode.HeaderOnlyWithRpc: + PublicDefinitions.Add("AIRLIB_HEADER_ONLY=1"); + AddLibDependency("AirLib", Path.Combine(AirLibPath, "lib"), "AirLib", Target, false); + LoadAirSimDependency(Target, "rpclib", "rpc"); + break; + + case CompileMode.CppCompileNoRpc: + LoadAirSimDependency(Target, "MavLinkCom", "MavLinkCom"); + PublicDefinitions.Add("AIRLIB_NO_RPC=1"); + break; + + case CompileMode.CppCompileWithRpc: + LoadAirSimDependency(Target, "rpclib", "rpc"); + break; + + default: + throw new System.Exception("CompileMode specified in plugin's Build.cs file is not recognized"); + } + + } + + public AirSim(ReadOnlyTargetRules Target) : base(Target) + { + //bEnforceIWYU = true; //to support 4.16 + PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; + + bEnableExceptions = true; + + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape", "CinematicCamera" }); + + // OpenHUTB 追加(2026-11-05):URovMuJoCoBridge 需要读取 MuJoCo 的 AMjArticulation / UMjBody, + // 以便把 AirSim 的 ROV Pawn 改造成 MuJoCo 的纯跟随者。 + // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + PublicDependencyModuleNames.Add("URLab"); + PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Foliage" }); + + // OpenHUTB 追加:ROV 配置文件(RovParams.json)解析需要 Json 模块。 + PrivateDependencyModuleNames.Add("Json"); + + string ProjectDir = Target.ProjectFile != null ? Path.GetDirectoryName(Target.ProjectFile.FullName) : ""; + string CarlaPluginInProject = Path.Combine(ProjectDir, "Plugins", "Carla"); + string CarlaPluginSibling = Path.Combine(AirSimPluginPath, "..", "Carla"); + bool bHaveCarla = Directory.Exists(CarlaPluginInProject) || Directory.Exists(CarlaPluginSibling); + + if (bHaveCarla) + { + PrivateDependencyModuleNames.Add("Carla"); + PublicDefinitions.Add("WITH_CARLA=1"); + } + else + { + PublicDefinitions.Add("WITH_CARLA=0"); + } + + //suppress VC++ proprietary warnings + PublicDefinitions.Add("_SCL_SECURE_NO_WARNINGS=1"); + PublicDefinitions.Add("_CRT_SECURE_NO_WARNINGS=1"); + PublicDefinitions.Add("HMD_MODULE_INCLUDED=0"); + PublicDefinitions.Add("NOMINMAX"); + + PublicIncludePaths.Add(Path.Combine(AirLibPath, "include")); + PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", "eigen3")); + PublicIncludePaths.Add(Path.Combine(ModulePath, "MavLinkCom", "include")); + PublicIncludePaths.Add(Path.Combine(ModulePath, "MavLinkCom", "common_utils")); + AddOSLibDependencies(Target); + + SetupCompileMode(CompileMode.HeaderOnlyWithRpc, Target); + } + + private void AddOSLibDependencies(ReadOnlyTargetRules Target) + { + if (Target.Platform == UnrealTargetPlatform.Win64) + { + // for SHGetFolderPath. + PublicAdditionalLibraries.Add("Shell32.lib"); + + //for joystick support + PublicAdditionalLibraries.Add("dinput8.lib"); + PublicAdditionalLibraries.Add("dxguid.lib"); + } + + if (Target.Platform == UnrealTargetPlatform.Linux) + { + // needed when packaging + PublicAdditionalLibraries.Add("stdc++"); + PublicAdditionalLibraries.Add("supc++"); + } + } + + static void CopyFileIfNewer(string srcFilePath, string destFolder) + { + FileInfo srcFile = new FileInfo(srcFilePath); + FileInfo destFile = new FileInfo(Path.Combine(destFolder, srcFile.Name)); + if (!destFile.Exists || srcFile.LastWriteTime > destFile.LastWriteTime) + { + srcFile.CopyTo(destFile.FullName, true); + } + //else skip + } + + private bool LoadAirSimDependency(ReadOnlyTargetRules Target, string LibName, string LibFileName) + { + string LibrariesPath = Path.Combine(AirLibPath, "deps", LibName, "lib"); + return AddLibDependency(LibName, LibrariesPath, LibFileName, Target, true); + } + + private bool AddLibDependency(string LibName, string LibPath, string LibFileName, ReadOnlyTargetRules Target, bool IsAddLibInclude) + { + string PlatformString = (Target.Platform == UnrealTargetPlatform.Win64 || Target.Platform == UnrealTargetPlatform.Mac) ? "x64" : "x86"; + string ConfigurationString = (Target.Configuration == UnrealTargetConfiguration.Debug) ? "Debug" : "Release"; + bool isLibrarySupported = false; + + + if (Target.Platform == UnrealTargetPlatform.Win64) + { + isLibrarySupported = true; + + PublicAdditionalLibraries.Add(Path.Combine(LibPath, PlatformString, ConfigurationString, LibFileName + ".lib")); + } else if (Target.Platform == UnrealTargetPlatform.Linux || Target.Platform == UnrealTargetPlatform.Mac) { + isLibrarySupported = true; + PublicAdditionalLibraries.Add(Path.Combine(LibPath, "lib" + LibFileName + ".a")); + } + + if (isLibrarySupported && IsAddLibInclude) + { + // Include path + PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", LibName, "include")); + } + PublicDefinitions.Add(string.Format("WITH_" + LibName.ToUpper() + "_BINDING={0}", isLibrarySupported ? 1 : 0)); + + return isLibrarySupported; + } +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/PawnSimApi.h new file mode 100644 index 0000000000..e33b0c91d0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/PawnSimApi.h @@ -0,0 +1,212 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Pawn.h" +#include "Particles/ParticleSystemComponent.h" +#include "UnrealImageCapture.h" + +#include +#include +#include "common/Common.hpp" +#include "common/common_utils/Signal.hpp" +#include "common/CommonStructs.hpp" +#include "common/GeodeticConverter.hpp" +#include "PIPCamera.h" +#include "physics/Kinematics.hpp" +#include "NedTransform.h" +#include "common/AirSimSettings.hpp" +#include "SimJoyStick/SimJoyStick.h" +#include "api/VehicleApiBase.hpp" +#include "api/VehicleSimApiBase.hpp" +#include "common/common_utils/UniqueValueMap.hpp" + +#include "PawnEvents.h" + +class PawnSimApi : public msr::airlib::VehicleSimApiBase +{ +public: //types + typedef msr::airlib::GeoPoint GeoPoint; + typedef msr::airlib::Vector2r Vector2r; + typedef msr::airlib::Vector3r Vector3r; + typedef msr::airlib::Pose Pose; + typedef msr::airlib::Quaternionr Quaternionr; + typedef msr::airlib::CollisionInfo CollisionInfo; + typedef msr::airlib::VectorMath VectorMath; + typedef msr::airlib::real_T real_T; + typedef msr::airlib::Utils Utils; + typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting; + typedef msr::airlib::ImageCaptureBase ImageCaptureBase; + typedef msr::airlib::DetectionInfo DetectionInfo; + typedef msr::airlib::Kinematics Kinematics; + + struct Params + { + APawn* pawn; + const NedTransform* global_transform; + PawnEvents* pawn_events; + common_utils::UniqueValueMap cameras; + UClass* pip_camera_class; + UParticleSystem* collision_display_template; + msr::airlib::GeoPoint home_geopoint; + std::string vehicle_name; + + Params() + { + } + + Params(APawn* pawn_val, const NedTransform* global_transform_val, PawnEvents* pawn_events_val, + const common_utils::UniqueValueMap& cameras_val, UClass* pip_camera_class_val, + UParticleSystem* collision_display_template_val, const msr::airlib::GeoPoint& home_geopoint_val, + const std::string& vehicle_name_val) + : pawn(pawn_val) + , global_transform(global_transform_val) + , pawn_events(pawn_events_val) + , cameras(cameras_val) + , pip_camera_class(pip_camera_class_val) + , collision_display_template(collision_display_template_val) + , home_geopoint(home_geopoint_val) + , vehicle_name(vehicle_name_val) + { + } + }; + +public: //implementation of VehicleSimApiBase + virtual void initialize() override; + + virtual void resetImplementation() override; + virtual void update() override; + + virtual const UnrealImageCapture* getImageCapture() const override; + virtual Pose getPose() const override; + virtual void setPose(const Pose& pose, bool ignore_collision) override; + + virtual CollisionInfo getCollisionInfo() const override; + virtual CollisionInfo getCollisionInfoAndReset() override; + virtual int getRemoteControlID() const override; + virtual msr::airlib::RCData getRCData() const override; + virtual std::string getVehicleName() const override + { + return params_.vehicle_name; + } + virtual void toggleTrace() override; + virtual void setTraceLine(const std::vector& color_rgba, float thickness) override; + + virtual void updateRenderedState(float dt) override; + virtual void updateRendering(float dt) override; + virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override; + virtual void setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) override; + virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override; + virtual std::string getRecordFileLine(bool is_header_line) const override; + virtual void reportState(msr::airlib::StateReporter& reporter) override; + +protected: //additional interface for derived class + virtual void pawnTick(float dt); + void setPoseInternal(const Pose& pose, bool ignore_collision); + virtual msr::airlib::VehicleApiBase* getVehicleApiBase() const; + msr::airlib::Kinematics* getKinematics(); + msr::airlib::Environment* getEnvironment(); + + // OpenHUTB 追加(2026-11-05):把 UE 世界坐标转成 AirSim 的 NED Pose。 + // 派生类(RovPawnSimApi)需要它来接收外部物理引擎(MuJoCo)提供的位姿, + // 保证坐标转换只有一处实现——复用已有的 private toPose()。 + // 依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + PawnSimApi::Pose toPoseFromUnreal(const FVector& u_position, const FQuat& u_quat) const + { + return toPose(u_position, u_quat); + } + +public: //Unreal specific methods + PawnSimApi(const Params& params); + + //returns one of the cameras attached to the pawn + const APIPCamera* getCamera(const std::string& camera_name) const; + APIPCamera* getCamera(const std::string& camera_name); + int getCameraCount(); + + virtual bool testLineOfSightToPoint(const msr::airlib::GeoPoint& point) const; + + //if enabled, this would show some flares + void displayCollisionEffect(FVector hit_location, const FHitResult& hit); + + //return the attached pawn + APawn* getPawn(); + + //get/set pose + //parameters in NED frame + void setDebugPose(const Pose& debug_pose); + + FVector getUUPosition() const; + FRotator getUUOrientation() const; + + const NedTransform& getNedTransform() const; + + void possess(); + void setRCForceFeedback(float rumble_strength, float auto_center); + +private: //methods + bool canTeleportWhileMove() const; + void allowPassthroughToggleInput(); + void detectUsbRc(); + void setupCamerasFromSettings(const common_utils::UniqueValueMap& cameras); + void createCamerasFromSettings(); + //on collision, pawns should update this + void onCollision(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, + bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit); + + //these methods are for future usage + void plot(std::istream& s, FColor color, const Vector3r& offset); + PawnSimApi::Pose toPose(const FVector& u_position, const FQuat& u_quat) const; + void updateKinematics(float dt); + void setStartPosition(const FVector& position, const FRotator& rotator); + +private: //vars + typedef msr::airlib::AirSimSettings AirSimSettings; + typedef msr::airlib::Environment Environment; + + Params params_; + common_utils::UniqueValueMap cameras_; + msr::airlib::GeoPoint home_geo_point_; + + std::string vehicle_name_; + NedTransform ned_transform_; + + FVector ground_trace_end_; + FVector ground_margin_; + std::unique_ptr image_capture_; + std::string log_line_; + + mutable msr::airlib::RCData rc_data_; + mutable SimJoyStick joystick_; + mutable SimJoyStick::State joystick_state_; + +protected: //state (accessible by subclasses for manual collision injection) + struct State + { + FVector start_location; + FRotator start_rotation; + FVector last_position; + FVector last_debug_position; + FVector current_position; + FVector current_debug_position; + FVector debug_position_offset; + bool tracing_enabled; + bool collisions_enabled; + bool passthrough_enabled; + bool was_last_move_teleport; + CollisionInfo collision_info; + + FVector mesh_origin; + FVector mesh_bounds; + FVector ground_offset; + FVector transformation_offset; + }; + + State state_, initial_state_; + +private: + std::unique_ptr kinematics_; + std::unique_ptr environment_; + + FColor trace_color_ = FColor::Purple; + float trace_thickness_ = 3.0f; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/SimHUD/SimHUD.cpp new file mode 100644 index 0000000000..765daad110 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -0,0 +1,395 @@ +#include "SimHUD.h" +#include "UObject/ConstructorHelpers.h" +#include "Kismet/KismetSystemLibrary.h" +#include "Misc/FileHelper.h" +#include "HAL/PlatformProcess.h" + +#include "Vehicles/Multirotor/SimModeWorldMultiRotor.h" +#include "Vehicles/Car/SimModeCar.h" +#include "Vehicles/ComputerVision/SimModeComputerVision.h" +#include "Vehicles/Rov/SimModeWorldRov.h" + +#include "common/AirSimSettings.hpp" +#include + +ASimHUD::ASimHUD() +{ + static ConstructorHelpers::FClassFinder hud_widget_class(TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_SimHUDWidget'")); + widget_class_ = hud_widget_class.Succeeded() ? hud_widget_class.Class : nullptr; +} + +void ASimHUD::BeginPlay() +{ + Super::BeginPlay(); + + try { + UAirBlueprintLib::OnBeginPlay(); + loadLevel(); + + initializeSettings(); + setUnrealEngineSettings(); + createSimMode(); + createMainWidget(); + setupInputBindings(); + if (simmode_) + simmode_->startApiServer(); + } + catch (std::exception& ex) { + UAirBlueprintLib::LogMessageString("Error at startup: ", ex.what(), LogDebugLevel::Failure); + //FGenericPlatformMisc::PlatformInit(); + //FGenericPlatformMisc::MessageBoxExt(EAppMsgType::Ok, TEXT("Error at Startup"), ANSI_TO_TCHAR(ex.what())); + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, std::string("Error at startup: ") + ex.what(), "Error"); + } +} + +void ASimHUD::Tick(float DeltaSeconds) +{ + if (simmode_ && simmode_->EnableReport) + widget_->updateDebugReport(simmode_->getDebugReport()); +} + +void ASimHUD::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + if (simmode_) + simmode_->stopApiServer(); + + if (widget_) { + widget_->Destruct(); + widget_ = nullptr; + } + if (simmode_) { + simmode_->Destroy(); + simmode_ = nullptr; + } + + UAirBlueprintLib::OnEndPlay(); + + Super::EndPlay(EndPlayReason); +} + +void ASimHUD::toggleRecordHandler() +{ + simmode_->toggleRecording(); +} + +void ASimHUD::inputEventToggleRecording() +{ + toggleRecordHandler(); +} + +void ASimHUD::inputEventToggleReport() +{ + simmode_->EnableReport = !simmode_->EnableReport; + widget_->setReportVisible(simmode_->EnableReport); +} + +void ASimHUD::inputEventToggleHelp() +{ + widget_->toggleHelpVisibility(); +} + +void ASimHUD::inputEventToggleTrace() +{ + simmode_->toggleTraceAll(); +} + +void ASimHUD::updateWidgetSubwindowVisibility() +{ + for (int window_index = 0; window_index < AirSimSettings::kSubwindowCount; ++window_index) { + APIPCamera* camera = subwindow_cameras_[window_index]; + ImageType camera_type = getSubWindowSettings().at(window_index).image_type; + + bool is_visible = getSubWindowSettings().at(window_index).visible && camera != nullptr; + + if (camera != nullptr) { + camera->setCameraTypeEnabled(camera_type, is_visible); + //sub-window captures don't count as a request, set bCaptureEveryFrame and bCaptureOnMovement to display so we can show correctly the subwindow + camera->setCameraTypeUpdate(camera_type, false); + } + + widget_->setSubwindowVisibility(window_index, + is_visible, + is_visible ? camera->getRenderTarget(camera_type, false) : nullptr); + } +} + +bool ASimHUD::isWidgetSubwindowVisible(int window_index) +{ + return widget_->getSubwindowVisibility(window_index) != 0; +} + +void ASimHUD::toggleSubwindowVisibility(int window_index) +{ + getSubWindowSettings().at(window_index).visible = !getSubWindowSettings().at(window_index).visible; + updateWidgetSubwindowVisibility(); +} + +void ASimHUD::inputEventToggleSubwindow0() +{ + toggleSubwindowVisibility(0); +} + +void ASimHUD::inputEventToggleSubwindow1() +{ + toggleSubwindowVisibility(1); +} + +void ASimHUD::inputEventToggleSubwindow2() +{ + toggleSubwindowVisibility(2); +} + +void ASimHUD::inputEventToggleAll() +{ + getSubWindowSettings().at(0).visible = !getSubWindowSettings().at(0).visible; + getSubWindowSettings().at(1).visible = getSubWindowSettings().at(2).visible = getSubWindowSettings().at(0).visible; + updateWidgetSubwindowVisibility(); +} + +void ASimHUD::createMainWidget() +{ + //create main widget + if (widget_class_ != nullptr) { + APlayerController* player_controller = this->GetWorld()->GetFirstPlayerController(); + auto* pawn = player_controller->GetPawn(); + if (pawn) { + std::string pawn_name = std::string(TCHAR_TO_ANSI(*pawn->GetName())); + Utils::log(pawn_name); + } + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, std::string("There were no compatible vehicles created for current SimMode! Check your settings.json."), "Error"); + UAirBlueprintLib::LogMessage(TEXT("There were no compatible vehicles created for current SimMode! Check your settings.json."), TEXT(""), LogDebugLevel::Failure); + } + + widget_ = CreateWidget(player_controller, widget_class_); + } + else { + widget_ = nullptr; + UAirBlueprintLib::LogMessage(TEXT("Cannot instantiate BP_SimHUDWidget blueprint!"), TEXT(""), LogDebugLevel::Failure); + } + + initializeSubWindows(); + + widget_->AddToViewport(); + + //synchronize PIP views + widget_->initializeForPlay(); + if (simmode_) + widget_->setReportVisible(simmode_->EnableReport); + widget_->setOnToggleRecordingHandler(std::bind(&ASimHUD::toggleRecordHandler, this)); + widget_->setRecordButtonVisibility(AirSimSettings::singleton().is_record_ui_visible); + updateWidgetSubwindowVisibility(); +} + +void ASimHUD::setUnrealEngineSettings() +{ + //TODO: should we only do below on SceneCapture2D components and cameras? + //avoid motion blur so capture images don't get + GetWorld()->GetGameViewport()->GetEngineShowFlags()->SetMotionBlur(false); + + //use two different methods to set console var because sometime it doesn't seem to work + static const auto custom_depth_var = IConsoleManager::Get().FindConsoleVariable(TEXT("r.CustomDepth")); + custom_depth_var->Set(3); + + //Equivalent to enabling Custom Stencil in Project > Settings > Rendering > Postprocessing + UKismetSystemLibrary::ExecuteConsoleCommand(GetWorld(), FString("r.CustomDepth 3")); + + //during startup we init stencil IDs to random hash and it takes long time for large environments + //we get error that GameThread has timed out after 30 sec waiting on render thread + static const auto render_timeout_var = IConsoleManager::Get().FindConsoleVariable(TEXT("g.TimeoutForBlockOnRenderFence")); + render_timeout_var->Set(300000); +} + +void ASimHUD::setupInputBindings() +{ + UAirBlueprintLib::EnableInput(this); + + UAirBlueprintLib::BindActionToKey("inputEventToggleRecording", EKeys::R, this, &ASimHUD::inputEventToggleRecording); + UAirBlueprintLib::BindActionToKey("InputEventToggleReport", EKeys::Semicolon, this, &ASimHUD::inputEventToggleReport); + UAirBlueprintLib::BindActionToKey("InputEventToggleHelp", EKeys::F1, this, &ASimHUD::inputEventToggleHelp); + UAirBlueprintLib::BindActionToKey("InputEventToggleTrace", EKeys::T, this, &ASimHUD::inputEventToggleTrace); + + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow0", EKeys::One, this, &ASimHUD::inputEventToggleSubwindow0); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow1", EKeys::Two, this, &ASimHUD::inputEventToggleSubwindow1); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow2", EKeys::Three, this, &ASimHUD::inputEventToggleSubwindow2); + UAirBlueprintLib::BindActionToKey("InputEventToggleAll", EKeys::Zero, this, &ASimHUD::inputEventToggleAll); +} + +void ASimHUD::initializeSettings() +{ + std::string settingsText; + if (getSettingsText(settingsText)) + AirSimSettings::initializeSettings(settingsText); + else + AirSimSettings::createDefaultSettingsFile(); + + AirSimSettings::singleton().load(std::bind(&ASimHUD::getSimModeFromUser, this)); + for (const auto& warning : AirSimSettings::singleton().warning_messages) { + UAirBlueprintLib::LogMessageString(warning, "", LogDebugLevel::Failure); + } + for (const auto& error : AirSimSettings::singleton().error_messages) { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, error, "settings.json"); + } +} + +const std::vector& ASimHUD::getSubWindowSettings() const +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::vector& ASimHUD::getSubWindowSettings() +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::string ASimHUD::getSimModeFromUser() +{ + if (EAppReturnType::No == UAirBlueprintLib::ShowMessage(EAppMsgType::YesNo, + "Would you like to use car simulation? Choose no to use quadrotor simulation.", + "Choose Vehicle")) { + return AirSimSettings::kSimModeTypeMultirotor; + } + else + return AirSimSettings::kSimModeTypeCar; +} + +void ASimHUD::loadLevel() +{ + if (AirSimSettings::singleton().level_name != "") + UAirBlueprintLib::RunCommandOnGameThread([&]() { UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str())); }, true); + else + UAirBlueprintLib::RunCommandOnGameThread([&]() { UAirBlueprintLib::loadLevel(this->GetWorld(), FString("Blocks")); }, true); +} + +void ASimHUD::createSimMode() +{ + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + FActorSpawnParameters simmode_spawn_params; + simmode_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + //spawn at origin. We will use this to do global NED transforms, for ex, non-vehicle objects in environment + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor) + simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, + FRotator::ZeroRotator, + simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeCar) + simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, + FRotator::ZeroRotator, + simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeComputerVision) + simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, + FRotator::ZeroRotator, + simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeRov) + simmode_ = this->GetWorld()->SpawnActor(FVector::ZeroVector, + FRotator::ZeroRotator, + simmode_spawn_params); + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, std::string("SimMode is not valid: ") + simmode_name, "Error"); + UAirBlueprintLib::LogMessageString("SimMode is not valid: ", simmode_name, LogDebugLevel::Failure); + } +} + +void ASimHUD::initializeSubWindows() +{ + if (!simmode_) + return; + + auto default_vehicle_sim_api = simmode_->getVehicleSimApi(); + + if (default_vehicle_sim_api) { + auto camera_count = default_vehicle_sim_api->getCameraCount(); + + //setup defaults + if (camera_count > 0) { + subwindow_cameras_[0] = default_vehicle_sim_api->getCamera(""); + subwindow_cameras_[1] = default_vehicle_sim_api->getCamera(""); //camera_count > 3 ? 3 : 0 + subwindow_cameras_[2] = default_vehicle_sim_api->getCamera(""); //camera_count > 4 ? 4 : 0 + } + else + subwindow_cameras_[0] = subwindow_cameras_[1] = subwindow_cameras_[2] = nullptr; + } + + for (const auto& setting : getSubWindowSettings()) { + APIPCamera* camera = simmode_->getCamera(msr::airlib::CameraDetails(setting.camera_name, setting.vehicle_name, setting.external)); + if (camera) + subwindow_cameras_[setting.window_index] = camera; + else + UAirBlueprintLib::LogMessageString("Invalid Camera settings in element", + std::to_string(setting.window_index), + LogDebugLevel::Failure); + } +} + +FString ASimHUD::getLaunchPath(const std::string& filename) +{ + FString launch_rel_path = FPaths::LaunchDir(); + FString abs_path = FPaths::ConvertRelativePathToFull(launch_rel_path); + return FPaths::Combine(abs_path, FString(filename.c_str())); +} + +// Attempts to parse the settings text from one of multiple locations. +// First, check the command line for settings provided via "-s" or "--settings" arguments +// Next, check the executable's working directory for the settings file. +// Finally, check the user's documents folder. +// If the settings file cannot be read, throw an exception + +bool ASimHUD::getSettingsText(std::string& settingsText) +{ + return (getSettingsTextFromCommandLine(settingsText) || + readSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || + // Use UE native module directory API to avoid encoding conversion and ensureFolder mkdir side effects + readSettingsTextFromFile(FPaths::Combine(FPlatformProcess::GetModulesDirectory(), TEXT("settings.json")), settingsText) || + readSettingsTextFromFile(getLaunchPath("settings.json"), settingsText) || + readSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); +} + +// Attempts to parse the settings file path or the settings text from the command line +// Looks for the flag "-settings=". If it exists, settingsText will be set to the value. +// Example (Path): AirSim.exe -settings="C:\path\to\settings.json" +// Example (Text): AirSim.exe -settings={"foo":"bar"} -> settingsText will be set to {"foo":"bar"} +// Returns true if the argument is present, false otherwise. +bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) +{ + + bool found = false; + FString settingsTextFString; + const TCHAR* commandLineArgs = FCommandLine::Get(); + + if (FParse::Param(commandLineArgs, TEXT("-settings"))) { + FString commandLineArgsFString = FString(commandLineArgs); + int idx = commandLineArgsFString.Find(TEXT("-settings")); + FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (readSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { + return true; + } + + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { + settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); + found = true; + } + } + + return found; +} + +bool ASimHUD::readSettingsTextFromFile(const FString& settingsFilepath, std::string& settingsText) +{ + bool found = FPaths::FileExists(settingsFilepath); + if (found) { + FString settingsTextFStr; + bool readSuccessful = FFileHelper::LoadFileToString(settingsTextFStr, *settingsFilepath); + if (readSuccessful) { + UAirBlueprintLib::LogMessageString("Loaded settings from ", TCHAR_TO_UTF8(*settingsFilepath), LogDebugLevel::Informational); + settingsText = TCHAR_TO_UTF8(*settingsTextFStr); + } + else { + UAirBlueprintLib::LogMessageString("Cannot read file ", TCHAR_TO_UTF8(*settingsFilepath), LogDebugLevel::Failure); + throw std::runtime_error("Cannot read settings file."); + } + } + + return found; +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/SimWorldGameMode.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/SimWorldGameMode.cpp new file mode 100644 index 0000000000..95812d664e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/SimWorldGameMode.cpp @@ -0,0 +1,1242 @@ +// SimWorldGameMode.cpp — Unified CARLA + AirSim GameMode +// All methods ported from SimHUD.cpp and AirSimGameMode.cpp with minimal changes. +// v0.1.5: Scroll wheel speed, physics/invincible toggle (P), help overlay (H). + +#include "SimWorldGameMode.h" +#include "UObject/ConstructorHelpers.h" +#include "Kismet/KismetSystemLibrary.h" +#include "Misc/FileHelper.h" +#include "HAL/PlatformProcess.h" +#include "IImageWrapperModule.h" + +#include "Vehicles/Multirotor/SimModeWorldMultiRotor.h" +#include "Vehicles/Multirotor/MultirotorPawnSimApi.h" +#include "Vehicles/Car/SimModeCar.h" +#include "Vehicles/ComputerVision/SimModeComputerVision.h" +#include "Vehicles/Rov/SimModeWorldRov.h" + +#include "common/AirSimSettings.hpp" +#include "common/Common.hpp" +#include "AirBlueprintLib.h" +#include "Carla/Game/CarlaHUD.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/Weather/Weather.h" +#include "Carla/Actor/CarlaActorFactory.h" +#include "Carla/Actor/CarlaActorFactoryBlueprint.h" +#include "Carla/Sensor/SensorFactory.h" +#include "Carla/Actor/StaticMeshFactory.h" +#include "Carla/Trigger/TriggerFactory.h" +#include "Carla/Actor/UtilActorFactory.h" +#include "Carla/AI/AIControllerFactory.h" +#include "Carla/Actor/ActorDispatcher.h" +#include "Carla/Game/CarlaGameInstance.h" + +#include "GameFramework/SpectatorPawn.h" +#include "Kismet/GameplayStatics.h" + +#include "Widgets/SOverlay.h" +#include "Widgets/SBoxPanel.h" +#include "Widgets/Layout/SBorder.h" +#include "Widgets/Layout/SSpacer.h" +#include "Widgets/Text/STextBlock.h" +#include "Styling/CoreStyle.h" + +#include +#include + +// ---------- AirSim logger (separate instance for SimWorldGameMode) ---------- + +class ASimWorldUnrealLog : public msr::airlib::Utils::Logger +{ +public: + virtual void log(int level, const std::string& message) override + { + size_t tab_pos; + static const std::string delim = ":\t"; + if ((tab_pos = message.find(delim)) != std::string::npos) { + UAirBlueprintLib::LogMessageString(message.substr(0, tab_pos), + message.substr(tab_pos + delim.size(), std::string::npos), + LogDebugLevel::Informational); + return; + } + + if (level == msr::airlib::Utils::kLogLevelError) { + UE_LOG(LogTemp, Error, TEXT("%s"), *FString(message.c_str())); + } + else if (level == msr::airlib::Utils::kLogLevelWarn) { + UE_LOG(LogTemp, Warning, TEXT("%s"), *FString(message.c_str())); + } + else { + UE_LOG(LogTemp, Log, TEXT("%s"), *FString(message.c_str())); + } + + msr::airlib::Utils::Logger::log(level, message); + } +}; + +static ASimWorldUnrealLog GlobalSimWorldLog; + +// ========================== FDroneControlWorker ========================== +// Background thread: reads shared velocity/yaw, calls blocking moveByVelocity() + +class FDroneControlWorker : public FRunnable +{ +public: + FDroneControlWorker(ASimModeBase* SimMode, FCriticalSection* Lock, + FVector* VelocityNED, float* Yaw, bool* ShouldHover, bool* DroneReady) + : SimMode_(SimMode), Lock_(Lock), VelocityNED_(VelocityNED), Yaw_(Yaw), bShouldHover_(ShouldHover), bDroneReady_(DroneReady) + { + } + + virtual bool Init() override + { + bRunning_ = true; + return true; + } + + virtual uint32 Run() override + { + using namespace msr::airlib; + + // Get the multirotor API + auto* PawnApi = SimMode_ ? SimMode_->getVehicleSimApi() : nullptr; + if (!PawnApi) { + UE_LOG(LogTemp, Error, TEXT("FDroneControlWorker: No vehicle sim API!")); + return 1; + } + + auto* MultiPawnApi = static_cast(PawnApi); + MultirotorApiBase* DroneApi = MultiPawnApi->getVehicleApi(); + if (!DroneApi) { + UE_LOG(LogTemp, Error, TEXT("FDroneControlWorker: No multirotor API!")); + return 1; + } + + // Don't call enableApiControl/armDisarm at startup — this allows + // external Python API to control the drone freely. FPS control will + // activate on first keyboard input. + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: Ready. Use WASD/QE for FPS control, or Python API for scripted flight.")); + + *bDroneReady_ = true; + bool bWasHovering = true; // true = don't hover on startup + bool bFPSActivated = false; // becomes true after first keyboard input + + // Main control loop + while (bRunning_) { + FVector Vel; + float YawCmd; + bool Hover; + + // Read shared state + { + FScopeLock ScopeLock(Lock_); + Vel = *VelocityNED_; + YawCmd = *Yaw_; + Hover = *bShouldHover_; + } + + // If no keyboard input has ever been detected, just sleep (let Python API work) + if (!bFPSActivated && Hover) { + FPlatformProcess::Sleep(0.1f); + continue; + } + + try { + // First keyboard/mouse input: take API control + if (!bFPSActivated && !Hover) { + bFPSActivated = true; + DroneApi->enableApiControl(true); + DroneApi->armDisarm(true); + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: FPS control activated!")); + } + + if (bFPSActivated) { + // Always send velocity + yaw (even when hovering) + // This ensures yaw updates even without WASD input + YawMode yaw_mode(false, YawCmd); + DroneApi->moveByVelocity( + Hover ? 0.0f : Vel.X, + Hover ? 0.0f : Vel.Y, + Hover ? 0.0f : Vel.Z, + 0.1f, + DrivetrainType::MaxDegreeOfFreedom, + yaw_mode); + } + else { + // Not yet activated: sleep and let Python API work + FPlatformProcess::Sleep(0.05f); + } + } + catch (const std::exception& ex) { + UE_LOG(LogTemp, Warning, TEXT("FDroneControlWorker: %s"), *FString(ex.what())); + FPlatformProcess::Sleep(0.1f); + } + } + + // Cleanup on shutdown (only if FPS was activated) + if (bFPSActivated) { + UE_LOG(LogTemp, Log, TEXT("FDroneControlWorker: Landing...")); + try { + DroneApi->hover(); + DroneApi->land(10.0f); + DroneApi->armDisarm(false); + DroneApi->enableApiControl(false); + } + catch (const std::exception& ex) { + UE_LOG(LogTemp, Warning, TEXT("FDroneControlWorker: Landing error: %s"), *FString(ex.what())); + } + } + + return 0; + } + + virtual void Stop() override + { + bRunning_ = false; + } + + virtual void Exit() override + { + } + +private: + ASimModeBase* SimMode_ = nullptr; + FCriticalSection* Lock_ = nullptr; + FVector* VelocityNED_ = nullptr; + float* Yaw_ = nullptr; + bool* bShouldHover_ = nullptr; + bool* bDroneReady_ = nullptr; + volatile bool bRunning_ = false; +}; + +// ========================== Constructor ========================== + +ASimWorldGameMode::ASimWorldGameMode(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) // CARLA creates Episode, Recorder, TaggerDelegate, etc. +{ + // AirSim manages its own vehicles; CARLA spawns via ActorFactories. + // Setting DefaultPawnClass = nullptr prevents UE4 from auto-spawning a pawn + // that would break AirSim drone control. + // We manually create a spectator pawn in BeginPlay() for CARLA compatibility. + DefaultPawnClass = nullptr; + + // Keep HUDClass = ACarlaHUD from parent (AirSim UI uses UMG widget instead) + // HUDClass is already set to ACarlaHUD::StaticClass() by ACarlaGameModeBase constructor. + + // --- CARLA Blueprint properties (normally set in CarlaGameMode blueprint) --- + + // Weather blueprint class + static ConstructorHelpers::FClassFinder weather_class( + TEXT("/Game/Carla/Blueprints/Weather/BP_Weather")); + if (weather_class.Succeeded()) { + WeatherClass = weather_class.Class; + } + + // Actor Factories — C++ factories + ActorFactories.Add(ASensorFactory::StaticClass()); + ActorFactories.Add(AStaticMeshFactory::StaticClass()); + ActorFactories.Add(ATriggerFactory::StaticClass()); + ActorFactories.Add(AUtilActorFactory::StaticClass()); + ActorFactories.Add(AAIControllerFactory::StaticClass()); + + // Actor Factories — Blueprint factories (vehicles, walkers, props) + static ConstructorHelpers::FClassFinder vehicle_factory( + TEXT("/Game/Carla/Blueprints/Vehicles/VehicleFactory")); + if (vehicle_factory.Succeeded()) + ActorFactories.Add(vehicle_factory.Class); + + static ConstructorHelpers::FClassFinder walker_factory( + TEXT("/Game/Carla/Blueprints/Walkers/WalkerFactory")); + if (walker_factory.Succeeded()) + ActorFactories.Add(walker_factory.Class); + + static ConstructorHelpers::FClassFinder prop_factory( + TEXT("/Game/Carla/Blueprints/Props/PropFactory")); + if (prop_factory.Succeeded()) + ActorFactories.Add(prop_factory.Class); + + // --- AirSim setup --- + + // Initialize AirSim logger + common_utils::Utils::getSetLogger(&GlobalSimWorldLog); + + // Pre-load ImageWrapper module (must be done on main thread) + static IImageWrapperModule& ImageWrapperModule = + FModuleManager::LoadModuleChecked(TEXT("ImageWrapper")); + + // Load AirSim HUD widget blueprint + static ConstructorHelpers::FClassFinder hud_widget_class( + TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_SimHUDWidget'")); + WidgetClass_ = hud_widget_class.Succeeded() ? hud_widget_class.Class : nullptr; +} + +// ========================== BeginPlay ========================== + +void ASimWorldGameMode::BeginPlay() +{ + // Full CARLA initialization: Episode, Weather, Traffic, Recorder, etc. + Super::BeginPlay(); + + // Manually create a spectator pawn for CARLA API compatibility. + // DefaultPawnClass is nullptr (required for AirSim drone control), so + // Episode->InitializeAtBeginPlay() could not find a spectator. + // We create one now and register it with CARLA's Episode. + // IMPORTANT: Do NOT possess the pawn — possessing it breaks AirSim's input handling. + { + auto* World = GetWorld(); + if (World) { + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + auto* SpectatorPawn = World->SpawnActor( + ASpectatorPawn::StaticClass(), FTransform::Identity, SpawnParams); + if (SpectatorPawn) { + CachedSpectator_ = SpectatorPawn; + + // Register with CARLA Episode (get via GameInstance for non-const access) + auto* GI = Cast(GetGameInstance()); + auto* Episode = GI ? GI->GetCarlaEpisode() : nullptr; + if (Episode) { + Episode->Spectator = SpectatorPawn; + FActorDescription Description; + Description.Id = TEXT("spectator"); + Description.Class = SpectatorPawn->GetClass(); + Episode->ActorDispatcher->RegisterActor(*SpectatorPawn, Description); + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: Spectator pawn created and registered.")); + } + } + } + } + + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: CARLA BeginPlay complete, starting AirSim bootstrap...")); + + try { + UAirBlueprintLib::OnBeginPlay(); + + // NO loadLevel() — map already loaded by CARLA/UE4. AirSim's level_name setting is ignored. + + InitializeAirSimSettings(); + SetUnrealEngineSettings(); + CreateSimMode(); + CreateAirSimWidget(); + SetupAirSimInputBindings(); + + if (SimMode_) + SimMode_->startApiServer(); + + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: AirSim bootstrap complete. API server started.")); + + // Setup FPS drone control (must be after SimMode creation) + SetupFPSControl(); + } + catch (std::exception& ex) { + UAirBlueprintLib::LogMessageString("Error at AirSim startup: ", ex.what(), LogDebugLevel::Failure); + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("Error at AirSim startup: ") + ex.what(), + "Error"); + } +} + +// ========================== Tick ========================== + +void ASimWorldGameMode::Tick(float DeltaSeconds) +{ + // CARLA: Recorder tick + Super::Tick(DeltaSeconds); + + // One-time: register AirSim drone pawn with CARLA ActorDispatcher + // so Python scripts can find it via world.get_actors() + if (!bDroneRegistered_ && SimMode_) { + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (PawnApi) { + auto* DroneSimApi = static_cast(PawnApi); + APawn* DronePawn = DroneSimApi->getPawn(); + if (DronePawn) { + auto* GI = Cast(GetGameInstance()); + auto* Episode = GI ? GI->GetCarlaEpisode() : nullptr; + if (Episode) { + FActorDescription Description; + Description.Id = TEXT("airsim.drone"); + Description.Class = DronePawn->GetClass(); + Episode->ActorDispatcher->RegisterActor(*DronePawn, Description); + UE_LOG(LogTemp, Log, TEXT("SimWorldGameMode: Drone pawn registered with CARLA (type_id=airsim.drone)")); + } + bDroneRegistered_ = true; + } + } + } + + // AirSim: Update debug report widget + if (SimMode_ && SimMode_->EnableReport && Widget_) + Widget_->updateDebugReport(SimMode_->getDebugReport()); + + // FPS drone control: read input, update shared state + if (bFPSControlActive_) { + UpdateFPSControl(DeltaSeconds); + UpdateCameraFollow(); + if (bShowHelp_) + DrawHelpOverlay(); + } +} + +// ========================== EndPlay ========================== + +void ASimWorldGameMode::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + // Stop drone control thread + if (DroneWorker_) { + DroneWorker_->Stop(); + } + if (DroneThread_) { + DroneThread_->WaitForCompletion(); + delete DroneThread_; + DroneThread_ = nullptr; + } + if (DroneWorker_) { + delete DroneWorker_; + DroneWorker_ = nullptr; + } + bFPSControlActive_ = false; + bDroneReady_ = false; + + // Stop AirSim API server + if (SimMode_) + SimMode_->stopApiServer(); + + // Remove help overlay + if (HelpOverlayWrapper_.IsValid() && GEngine && GEngine->GameViewport) { + GEngine->GameViewport->RemoveViewportWidgetContent(HelpOverlayWrapper_.ToSharedRef()); + } + HelpOverlayWrapper_.Reset(); + HelpTitleBlock_.Reset(); + HelpSubtitleBlock_.Reset(); + HelpContentBlock_.Reset(); + HelpStatusBlock_.Reset(); + HelpOverlayContainer_.Reset(); + + // Destroy AirSim widget + if (Widget_) { + Widget_->Destruct(); + Widget_ = nullptr; + } + + // Destroy SimMode actor + if (SimMode_) { + SimMode_->Destroy(); + SimMode_ = nullptr; + } + + UAirBlueprintLib::OnEndPlay(); + + // CARLA cleanup: Episode->EndPlay(), GameInstance->NotifyEndEpisode(), etc. + Super::EndPlay(EndPlayReason); +} + +// ========================== FPS Drone Control ========================== + +void ASimWorldGameMode::SetupFPSControl() +{ + if (!SimMode_) { + UE_LOG(LogTemp, Warning, TEXT("SetupFPSControl: No SimMode, skipping FPS control.")); + return; + } + + // Only enable for multirotor mode + std::string simmode_name = AirSimSettings::singleton().simmode_name; + if (simmode_name != AirSimSettings::kSimModeTypeMultirotor) { + UE_LOG(LogTemp, Log, TEXT("SetupFPSControl: SimMode is not Multirotor, skipping FPS control.")); + return; + } + + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC) { + UE_LOG(LogTemp, Warning, TEXT("SetupFPSControl: No PlayerController!")); + return; + } + + // Set mouse capture for FPS control + PC->bShowMouseCursor = false; + FInputModeGameOnly InputMode; + PC->SetInputMode(InputMode); + + // Force viewport mouse lock — required since DefaultPawnClass=nullptr means + // no possessed pawn, and UE4's default mouse capture depends on pawn possession + if (UGameViewportClient* ViewportClient = GetWorld()->GetGameViewport()) { + ViewportClient->SetMouseLockMode(EMouseLockMode::LockAlways); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::CapturePermanently); + } + + // Set view target to spectator + if (CachedSpectator_) + PC->SetViewTarget(CachedSpectator_); + + // Build weather presets + { + // ClearNoon + FWeatherParameters w; + w.Cloudiness = 10.0f; + w.SunAltitudeAngle = 75.0f; + w.SunAzimuthAngle = 0.0f; + WeatherPresets_.Add(w); + } + { + // Cloudy + FWeatherParameters w; + w.Cloudiness = 80.0f; + w.SunAltitudeAngle = 50.0f; + WeatherPresets_.Add(w); + } + { + // Rain + FWeatherParameters w; + w.Cloudiness = 90.0f; + w.Precipitation = 80.0f; + w.PrecipitationDeposits = 60.0f; + w.Wetness = 80.0f; + w.SunAltitudeAngle = 40.0f; + WeatherPresets_.Add(w); + } + { + // Sunset + FWeatherParameters w; + w.Cloudiness = 30.0f; + w.SunAltitudeAngle = 5.0f; + w.SunAzimuthAngle = 270.0f; + WeatherPresets_.Add(w); + } + { + // Night + FWeatherParameters w; + w.Cloudiness = 20.0f; + w.SunAltitudeAngle = -80.0f; + WeatherPresets_.Add(w); + } + { + // DustStorm + FWeatherParameters w; + w.Cloudiness = 60.0f; + w.DustStorm = 80.0f; + w.FogDensity = 30.0f; + w.WindIntensity = 80.0f; + w.SunAltitudeAngle = 40.0f; + WeatherPresets_.Add(w); + } + + // Bind FPS control keys (true = fire on key press, not release) + UAirBlueprintLib::BindActionToKey("InputEventNextWeather", EKeys::N, this, &ASimWorldGameMode::InputEventNextWeather, true); + UAirBlueprintLib::BindActionToKey("InputEventToggleMouseCapture", EKeys::Tab, this, &ASimWorldGameMode::InputEventToggleMouseCapture, true); + UAirBlueprintLib::BindActionToKey("InputEventSpeedUp", EKeys::MouseScrollUp, this, &ASimWorldGameMode::InputEventSpeedUp, true); + UAirBlueprintLib::BindActionToKey("InputEventSpeedDown", EKeys::MouseScrollDown, this, &ASimWorldGameMode::InputEventSpeedDown, true); + UAirBlueprintLib::BindActionToKey("InputEventToggleHelpOverlay", EKeys::H, this, &ASimWorldGameMode::InputEventToggleHelpOverlay, true); + UAirBlueprintLib::BindActionToKey("InputEventTogglePhysicsMode", EKeys::P, this, &ASimWorldGameMode::InputEventTogglePhysicsMode, true); + + // Start drone control thread + DroneWorker_ = new FDroneControlWorker( + SimMode_, &DroneControlLock_, &DesiredVelocityNED_, &DesiredYaw_, &bShouldHover_, &bDroneReady_); + DroneThread_ = FRunnableThread::Create(DroneWorker_, TEXT("DroneControlThread")); + + bFPSControlActive_ = true; + UE_LOG(LogTemp, Log, TEXT("SetupFPSControl: FPS drone control active. WASD=Move, Mouse=Look, Scroll=Speed, N=Weather, H=Help, P=PhysicsToggle")); +} + +void ASimWorldGameMode::UpdateFPSControl(float DeltaSeconds) +{ + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC || !bDroneReady_) + return; + + // v0.1.3: Initialize FPSYaw_ from drone's actual orientation (once) + // Fixes W key flying in wrong direction on first press + if (!bYawInitialized_) { + auto* PawnApi = SimMode_ ? SimMode_->getVehicleSimApi() : nullptr; + if (PawnApi) { + FRotator DroneRot = PawnApi->getUUOrientation(); + FPSYaw_ = DroneRot.Yaw; + bYawInitialized_ = true; + } + } + + // ---- Mouse look: yaw only ---- + if (bMouseCaptured_) { + float MouseX = 0.0f, MouseY = 0.0f; + PC->GetInputMouseDelta(MouseX, MouseY); + + FPSYaw_ += MouseX * 0.35f; + + while (FPSYaw_ >= 360.0f) + FPSYaw_ -= 360.0f; + while (FPSYaw_ < 0.0f) + FPSYaw_ += 360.0f; + } + + // ---- Keyboard movement: horizontal + vertical ---- + // WASD = move on horizontal plane (yaw-relative), Space/Shift = up/down + float Fwd = 0.0f, Strafe = 0.0f, Vert = 0.0f; + if (PC->IsInputKeyDown(EKeys::W)) Fwd += 1.0f; + if (PC->IsInputKeyDown(EKeys::S)) Fwd -= 1.0f; + if (PC->IsInputKeyDown(EKeys::D)) Strafe += 1.0f; + if (PC->IsInputKeyDown(EKeys::A)) Strafe -= 1.0f; + if (PC->IsInputKeyDown(EKeys::SpaceBar)) Vert += 1.0f; // ascend + if (PC->IsInputKeyDown(EKeys::LeftShift)) Vert -= 1.0f; // descend + + bool bHasInput = (FMath::Abs(Fwd) > 0.01f || FMath::Abs(Strafe) > 0.01f || FMath::Abs(Vert) > 0.01f); + + // Horizontal velocity decomposed by yaw only (no pitch) + float YawRad = FMath::DegreesToRadians(FPSYaw_); + float VxNED = (Fwd * FMath::Cos(YawRad) - Strafe * FMath::Sin(YawRad)) * DroneSpeed_; + float VyNED = (Fwd * FMath::Sin(YawRad) + Strafe * FMath::Cos(YawRad)) * DroneSpeed_; + float VzNED = -Vert * DroneSpeed_; // NED: negative Z = up + + // Write to shared state + { + FScopeLock ScopeLock(&DroneControlLock_); + DesiredVelocityNED_ = FVector(VxNED, VyNED, VzNED); + DesiredYaw_ = FPSYaw_; + bShouldHover_ = !bHasInput; + } +} + +void ASimWorldGameMode::UpdateCameraFollow() +{ + if (!SimMode_ || !CachedSpectator_ || !bDroneReady_) + return; + + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (!PawnApi) + return; + + FVector DronePos = PawnApi->getUUPosition(); + FRotator DroneRot = PawnApi->getUUOrientation(); // drone's actual UE4 rotation + + // v0.1.3: Always 3rd person (removed 1st person toggle) + // 3rd person: fixed chase camera behind drone + FRotator YawOnly(0.0f, DroneRot.Yaw, 0.0f); + FVector Behind = YawOnly.Vector() * -800.0f; // 8m behind + Behind.Z += 350.0f; // 3.5m above + + FVector CamPos = DronePos + Behind; + FRotator LookAt = (DronePos - CamPos).Rotation(); + + CachedSpectator_->SetActorLocation(CamPos); + CachedSpectator_->SetActorRotation(LookAt); +} + +// ========================== FPS Input Handlers ========================== + +void ASimWorldGameMode::InputEventNextWeather() +{ + if (WeatherPresets_.Num() == 0) return; + + WeatherIndex_ = (WeatherIndex_ + 1) % WeatherPresets_.Num(); + const FWeatherParameters& Preset = WeatherPresets_[WeatherIndex_]; + + auto* Episode = UCarlaStatics::GetCurrentEpisode(GetWorld()); + if (Episode) { + AWeather* Weather = Episode->GetWeather(); + if (Weather) { + Weather->ApplyWeather(Preset); + } + } + + static const TCHAR* PresetNames[] = { + TEXT("ClearNoon"), TEXT("Cloudy"), TEXT("Rain"), TEXT("Sunset"), TEXT("Night"), TEXT("DustStorm") + }; + UE_LOG(LogTemp, Log, TEXT("Weather: %s"), PresetNames[WeatherIndex_]); +} + +void ASimWorldGameMode::InputEventToggleMouseCapture() +{ + APlayerController* PC = GetWorld()->GetFirstPlayerController(); + if (!PC) return; + + bMouseCaptured_ = !bMouseCaptured_; + UGameViewportClient* ViewportClient = GetWorld()->GetGameViewport(); + + if (bMouseCaptured_) { + PC->bShowMouseCursor = false; + FInputModeGameOnly InputMode; + PC->SetInputMode(InputMode); + if (ViewportClient) { + ViewportClient->SetMouseLockMode(EMouseLockMode::LockAlways); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::CapturePermanently); + } + } + else { + PC->bShowMouseCursor = true; + FInputModeGameAndUI InputMode; + InputMode.SetHideCursorDuringCapture(false); + PC->SetInputMode(InputMode); + if (ViewportClient) { + ViewportClient->SetMouseLockMode(EMouseLockMode::DoNotLock); + ViewportClient->SetCaptureMouseOnClick(EMouseCaptureMode::NoCapture); + } + } + UE_LOG(LogTemp, Log, TEXT("Mouse capture: %s"), bMouseCaptured_ ? TEXT("ON") : TEXT("OFF")); +} + +void ASimWorldGameMode::InputEventSpeedUp() +{ + DroneSpeed_ = FMath::Min(30.0f, DroneSpeed_ + 1.0f); + UE_LOG(LogTemp, Log, TEXT("Drone speed: %.0f m/s"), DroneSpeed_); +} + +void ASimWorldGameMode::InputEventSpeedDown() +{ + DroneSpeed_ = FMath::Max(1.0f, DroneSpeed_ - 1.0f); + UE_LOG(LogTemp, Log, TEXT("Drone speed: %.0f m/s"), DroneSpeed_); +} + +void ASimWorldGameMode::InputEventToggleHelpOverlay() +{ + bShowHelp_ = !bShowHelp_; + ShowHelpOverlay(bShowHelp_); + UE_LOG(LogTemp, Log, TEXT("Help overlay: %s"), bShowHelp_ ? TEXT("ON") : TEXT("OFF")); +} + +void ASimWorldGameMode::InputEventTogglePhysicsMode() +{ + bPhysicsCollision_ = !bPhysicsCollision_; + + // Push to MultirotorPawnSimApi + if (SimMode_) { + auto* PawnApi = SimMode_->getVehicleSimApi(); + if (PawnApi) { + auto* MultiPawnApi = static_cast(PawnApi); + MultiPawnApi->setPhysicsCollisionEnabled(bPhysicsCollision_); + } + } + + // On-screen notification (2s) + if (GEngine) { + FString Msg = bPhysicsCollision_ + ? TEXT("Physics Mode ON / 物理碰撞模式") + : TEXT("Invincible Mode ON / 无敌穿越模式"); + GEngine->AddOnScreenDebugMessage(7777, 2.0f, FColor::Yellow, Msg); + } + UE_LOG(LogTemp, Log, TEXT("Collision mode: %s"), bPhysicsCollision_ ? TEXT("Physics") : TEXT("Invincible")); +} + +void ASimWorldGameMode::DrawHelpOverlay() +{ + // Now handled by Slate widget — just update dynamic text + if (bShowHelp_ && HelpContentBlock_.IsValid()) { + UpdateHelpOverlayText(); + } +} + +void ASimWorldGameMode::CreateHelpOverlayWidget() +{ + if (HelpOverlayWrapper_.IsValid()) + return; + + // Font setup — DroidSansFallback for CJK support + FString FontPath = FPaths::EngineContentDir() / TEXT("Slate/Fonts/DroidSansFallback.ttf"); + FSlateFontInfo TitleFont(FontPath, 30); + TitleFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo SubtitleFont(FontPath, 16); + SubtitleFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo ContentFont(FontPath, 20); + ContentFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo StatusFont(FontPath, 18); + StatusFont.TypefaceFontName = FName("Bold"); + FSlateFontInfo KeyFont(FontPath, 18); + KeyFont.TypefaceFontName = FName("Bold"); + + // Color palette — high contrast, solid dark background + FLinearColor TitleColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor SubtitleColor(0.8f, 0.8f, 0.85f, 1.0f); + FLinearColor ContentColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor AccentColor(0.38f, 0.68f, 1.0f, 1.0f); // SF Blue + FLinearColor KeyColor(1.0f, 1.0f, 1.0f, 1.0f); + FLinearColor SeparatorColor(1.0f, 1.0f, 1.0f, 0.15f); + FLinearColor BgColor(0.0f, 0.0f, 0.0f, 0.95f); // near-opaque black + + // Title + HelpTitleBlock_ = SNew(STextBlock) + .Font(TitleFont) + .ColorAndOpacity(FSlateColor(TitleColor)) + .Justification(ETextJustify::Left); + + // Subtitle + HelpSubtitleBlock_ = SNew(STextBlock) + .Font(SubtitleFont) + .ColorAndOpacity(FSlateColor(SubtitleColor)) + .Justification(ETextJustify::Left); + + // Main content — with shadow for better readability + HelpContentBlock_ = SNew(STextBlock) + .Font(ContentFont) + .ColorAndOpacity(FSlateColor(ContentColor)) + .ShadowOffset(FVector2D(1.5f, 1.5f)) + .ShadowColorAndOpacity(FLinearColor(0.0f, 0.0f, 0.0f, 0.6f)) + .Justification(ETextJustify::Left); + + // Status bar — same white color as content, no blue + HelpStatusBlock_ = SNew(STextBlock) + .Font(StatusFont) + .ColorAndOpacity(FSlateColor(ContentColor)) + .ShadowOffset(FVector2D(1.5f, 1.5f)) + .ShadowColorAndOpacity(FLinearColor(0.0f, 0.0f, 0.0f, 0.6f)) + .Justification(ETextJustify::Left); + + // Thin separator line (horizontal rule) + auto MakeSeparator = [&SeparatorColor]() -> TSharedRef { + return SNew(SBorder) + .BorderBackgroundColor(SeparatorColor) + .Padding(0) + [SNew(SSpacer) + .Size(FVector2D(1.0f, 1.0f))]; + }; + + // Build layout — compact vertical stack, fits within 1080p viewport + HelpOverlayContainer_ = SNew(SVerticalBox) + // Title + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 2) + [HelpTitleBlock_.ToSharedRef()] + // Subtitle + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [HelpSubtitleBlock_.ToSharedRef()] + // Separator + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [MakeSeparator()] + // Content + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 10) + [HelpContentBlock_.ToSharedRef()] + // Separator + + SVerticalBox::Slot().AutoHeight().Padding(0, 0, 0, 8) + [MakeSeparator()] + // Status + + SVerticalBox::Slot().AutoHeight() + [HelpStatusBlock_.ToSharedRef()]; + + // Wrap in panel — SBox constrains max height to prevent viewport overflow + HelpOverlayWrapper_ = + SNew(SOverlay) + SOverlay::Slot() + .HAlign(HAlign_Left) + .VAlign(VAlign_Top) + .Padding(FMargin(36.0f, 36.0f, 0.0f, 0.0f)) + [SNew(SBox) + .MaxDesiredHeight(980.0f) + .Clipping(EWidgetClipping::ClipToBounds) + [SNew(SBorder) + .BorderBackgroundColor(BgColor) + .Padding(FMargin(28.0f, 22.0f, 36.0f, 20.0f)) + [HelpOverlayContainer_.ToSharedRef()]]]; + + UpdateHelpOverlayText(); +} + +void ASimWorldGameMode::UpdateHelpOverlayText() +{ + if (!HelpContentBlock_.IsValid()) + return; + + // Title + if (HelpTitleBlock_.IsValid()) + HelpTitleBlock_->SetText(FText::FromString(TEXT("CarlaAir"))); + + // Subtitle — v0.1.7 + if (HelpSubtitleBlock_.IsValid()) + HelpSubtitleBlock_->SetText(FText::FromString( + TEXT("v0.1.7 Air-ground integrated simulation platform"))); + // Chinese: 空地一体联合仿真平台 + + // Content — detailed bilingual help + FString Content = FString::Printf(TEXT( + "FLIGHT CONTROLS\n" + "\n" + " W / S Forward / Backward\n" + " A / D Strafe Left / Right\n" + " Mouse Yaw Turn Direction\n" + " Space Ascend Drone\n" + " Left Shift Descend Drone\n" + " Scroll Wheel Adjust flight speed (+/- 1 m/s)\n" + "\n" + "SYSTEM FUNCTIONS\n" + "\n" + " N Cycle Weather Presets\n" + " P Physics / Noclip\n" + " Tab Release / Capture Mouse\n" + " H Show / Hide Help\n" + " 1 / 2 / 3 Sensor Camera Views\n" + "\n" + "ADVANCED (for AirSim experts)\n" + "\n" + " I Toggle First-Person / Default View\n" + " B FPV Mode (mouse controls drone yaw)\n" + " FPV Mode (mouse controls drone yaw)\n" + " I / B For experienced AirSim users only")); + // Chinese decoded: 飞行控制, 前进/后退, 左移/右移, 偏航旋转方向, 上升无人机, 下降无人机, + // 调节飞行速度, 系统功能, 切换天气预设, 物理碰撞/穿越模式, 释放/捕获鼠标, 显示/隐藏帮助, + // 传感器画面, AirSim高级, 切换第一人称/默认视角, FPV模式(鼠标控制无人机偏航), + // 仅建议熟悉AirSim的用户使用 + HelpContentBlock_->SetText(FText::FromString(Content)); + + // Status line — white, same style as content + FString ModeStr = bPhysicsCollision_ + ? TEXT("Physics") + : TEXT("Noclip"); + + if (HelpStatusBlock_.IsValid()) { + FString Status = FString::Printf(TEXT( + "Speed: %.0f m/s | %s | Press H to close"), + DroneSpeed_, + *ModeStr); + // Chinese: 当前速度, 按H关闭 + HelpStatusBlock_->SetText(FText::FromString(Status)); + } +} + +void ASimWorldGameMode::ShowHelpOverlay(bool bShow) +{ + if (!GEngine || !GEngine->GameViewport) + return; + + if (bShow) { + CreateHelpOverlayWidget(); + if (HelpOverlayWrapper_.IsValid()) { + GEngine->GameViewport->AddViewportWidgetContent( + HelpOverlayWrapper_.ToSharedRef(), + 100 // z-order (above most UI) + ); + UpdateHelpOverlayText(); + } + } + else { + if (HelpOverlayWrapper_.IsValid()) { + GEngine->GameViewport->RemoveViewportWidgetContent( + HelpOverlayWrapper_.ToSharedRef()); + } + } +} + +// ========================== AirSim Settings ========================== + +void ASimWorldGameMode::InitializeAirSimSettings() +{ + std::string settingsText; + if (GetSettingsText(settingsText)) + AirSimSettings::initializeSettings(settingsText); + else + AirSimSettings::createDefaultSettingsFile(); + + AirSimSettings::singleton().load(std::bind(&ASimWorldGameMode::GetSimModeFromUser, this)); + + for (const auto& warning : AirSimSettings::singleton().warning_messages) { + UAirBlueprintLib::LogMessageString(warning, "", LogDebugLevel::Failure); + } + for (const auto& error : AirSimSettings::singleton().error_messages) { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, error, "settings.json"); + } +} + +void ASimWorldGameMode::SetUnrealEngineSettings() +{ + // Disable motion blur for clean capture images + GetWorld()->GetGameViewport()->GetEngineShowFlags()->SetMotionBlur(false); + + // Enable custom stencil (required for AirSim segmentation) + static const auto custom_depth_var = IConsoleManager::Get().FindConsoleVariable(TEXT("r.CustomDepth")); + custom_depth_var->Set(3); + UKismetSystemLibrary::ExecuteConsoleCommand(GetWorld(), FString("r.CustomDepth 3")); + + // Increase render fence timeout for large environments with stencil IDs + static const auto render_timeout_var = IConsoleManager::Get().FindConsoleVariable(TEXT("g.TimeoutForBlockOnRenderFence")); + render_timeout_var->Set(300000); +} + +// ========================== SimMode Creation ========================== + +void ASimWorldGameMode::CreateSimMode() +{ + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + FActorSpawnParameters simmode_spawn_params; + simmode_spawn_params.SpawnCollisionHandlingOverride = + ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + // Spawn at origin — used for global NED transforms + if (simmode_name == AirSimSettings::kSimModeTypeMultirotor) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeCar) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeComputerVision) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else if (simmode_name == AirSimSettings::kSimModeTypeRov) + SimMode_ = GetWorld()->SpawnActor( + FVector::ZeroVector, FRotator::ZeroRotator, simmode_spawn_params); + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("SimMode is not valid: ") + simmode_name, + "Error"); + UAirBlueprintLib::LogMessageString("SimMode is not valid: ", simmode_name, LogDebugLevel::Failure); + } +} + +// ========================== Widget / HUD ========================== + +void ASimWorldGameMode::CreateAirSimWidget() +{ + if (WidgetClass_ != nullptr) { + APlayerController* player_controller = GetWorld()->GetFirstPlayerController(); + auto* pawn = player_controller->GetPawn(); + if (pawn) { + std::string pawn_name = std::string(TCHAR_TO_ANSI(*pawn->GetName())); + Utils::log(pawn_name); + } + else { + UAirBlueprintLib::ShowMessage(EAppMsgType::Ok, + std::string("There were no compatible vehicles created for current SimMode! Check your settings.json."), + "Error"); + UAirBlueprintLib::LogMessage( + TEXT("There were no compatible vehicles created for current SimMode! Check your settings.json."), + TEXT(""), + LogDebugLevel::Failure); + } + + Widget_ = CreateWidget(player_controller, WidgetClass_); + } + else { + Widget_ = nullptr; + UAirBlueprintLib::LogMessage( + TEXT("Cannot instantiate BP_SimHUDWidget blueprint!"), TEXT(""), LogDebugLevel::Failure); + } + + InitializeSubWindows(); + + if (Widget_) { + Widget_->AddToViewport(); + Widget_->initializeForPlay(); + if (SimMode_) + Widget_->setReportVisible(SimMode_->EnableReport); + Widget_->setOnToggleRecordingHandler(std::bind(&ASimWorldGameMode::ToggleRecordHandler, this)); + Widget_->setRecordButtonVisibility(AirSimSettings::singleton().is_record_ui_visible); + UpdateWidgetSubwindowVisibility(); + } +} + +void ASimWorldGameMode::InitializeSubWindows() +{ + if (!SimMode_) + return; + + auto default_vehicle_sim_api = SimMode_->getVehicleSimApi(); + + if (default_vehicle_sim_api) { + auto camera_count = default_vehicle_sim_api->getCameraCount(); + + if (camera_count > 0) { + SubwindowCameras_[0] = default_vehicle_sim_api->getCamera(""); + SubwindowCameras_[1] = default_vehicle_sim_api->getCamera(""); + SubwindowCameras_[2] = default_vehicle_sim_api->getCamera(""); + } + else + SubwindowCameras_[0] = SubwindowCameras_[1] = SubwindowCameras_[2] = nullptr; + } + + for (const auto& setting : GetSubWindowSettings()) { + APIPCamera* camera = SimMode_->getCamera( + msr::airlib::CameraDetails(setting.camera_name, setting.vehicle_name, setting.external)); + if (camera) + SubwindowCameras_[setting.window_index] = camera; + else + UAirBlueprintLib::LogMessageString("Invalid Camera settings in element", + std::to_string(setting.window_index), + LogDebugLevel::Failure); + } +} + +// ========================== Input Bindings ========================== + +void ASimWorldGameMode::SetupAirSimInputBindings() +{ + UAirBlueprintLib::EnableInput(this); + + UAirBlueprintLib::BindActionToKey("inputEventToggleRecording", EKeys::R, this, &ASimWorldGameMode::InputEventToggleRecording); + UAirBlueprintLib::BindActionToKey("InputEventToggleReport", EKeys::Semicolon, this, &ASimWorldGameMode::InputEventToggleReport); + UAirBlueprintLib::BindActionToKey("InputEventToggleHelp", EKeys::F1, this, &ASimWorldGameMode::InputEventToggleHelp); + UAirBlueprintLib::BindActionToKey("InputEventToggleTrace", EKeys::T, this, &ASimWorldGameMode::InputEventToggleTrace); + + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow0", EKeys::One, this, &ASimWorldGameMode::InputEventToggleSubwindow0); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow1", EKeys::Two, this, &ASimWorldGameMode::InputEventToggleSubwindow1); + UAirBlueprintLib::BindActionToKey("InputEventToggleSubwindow2", EKeys::Three, this, &ASimWorldGameMode::InputEventToggleSubwindow2); + UAirBlueprintLib::BindActionToKey("InputEventToggleAll", EKeys::Zero, this, &ASimWorldGameMode::InputEventToggleAll); +} + +// ========================== Input Handlers ========================== + +void ASimWorldGameMode::ToggleRecordHandler() +{ + if (SimMode_) + SimMode_->toggleRecording(); +} + +void ASimWorldGameMode::InputEventToggleRecording() +{ + ToggleRecordHandler(); +} + +void ASimWorldGameMode::InputEventToggleReport() +{ + if (SimMode_ && Widget_) { + SimMode_->EnableReport = !SimMode_->EnableReport; + Widget_->setReportVisible(SimMode_->EnableReport); + } +} + +void ASimWorldGameMode::InputEventToggleHelp() +{ + if (Widget_) + Widget_->toggleHelpVisibility(); +} + +void ASimWorldGameMode::InputEventToggleTrace() +{ + if (SimMode_) + SimMode_->toggleTraceAll(); +} + +void ASimWorldGameMode::UpdateWidgetSubwindowVisibility() +{ + if (!Widget_) + return; + + for (int window_index = 0; window_index < AirSimSettings::kSubwindowCount; ++window_index) { + APIPCamera* camera = SubwindowCameras_[window_index]; + ImageType camera_type = GetSubWindowSettings().at(window_index).image_type; + + bool is_visible = GetSubWindowSettings().at(window_index).visible && camera != nullptr; + + if (camera != nullptr) { + camera->setCameraTypeEnabled(camera_type, is_visible); + camera->setCameraTypeUpdate(camera_type, false); + } + + Widget_->setSubwindowVisibility(window_index, + is_visible, + is_visible ? camera->getRenderTarget(camera_type, false) : nullptr); + } +} + +bool ASimWorldGameMode::IsWidgetSubwindowVisible(int window_index) +{ + return Widget_ ? Widget_->getSubwindowVisibility(window_index) != 0 : false; +} + +void ASimWorldGameMode::ToggleSubwindowVisibility(int window_index) +{ + GetSubWindowSettings().at(window_index).visible = !GetSubWindowSettings().at(window_index).visible; + UpdateWidgetSubwindowVisibility(); +} + +void ASimWorldGameMode::InputEventToggleSubwindow0() +{ + ToggleSubwindowVisibility(0); +} + +void ASimWorldGameMode::InputEventToggleSubwindow1() +{ + ToggleSubwindowVisibility(1); +} + +void ASimWorldGameMode::InputEventToggleSubwindow2() +{ + ToggleSubwindowVisibility(2); +} + +void ASimWorldGameMode::InputEventToggleAll() +{ + GetSubWindowSettings().at(0).visible = !GetSubWindowSettings().at(0).visible; + GetSubWindowSettings().at(1).visible = GetSubWindowSettings().at(2).visible = GetSubWindowSettings().at(0).visible; + UpdateWidgetSubwindowVisibility(); +} + +// ========================== Settings Helpers ========================== + +const std::vector& +ASimWorldGameMode::GetSubWindowSettings() const +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::vector& +ASimWorldGameMode::GetSubWindowSettings() +{ + return AirSimSettings::singleton().subwindow_settings; +} + +std::string ASimWorldGameMode::GetSimModeFromUser() +{ + if (EAppReturnType::No == UAirBlueprintLib::ShowMessage(EAppMsgType::YesNo, + "Would you like to use car simulation? Choose no to use quadrotor simulation.", + "Choose Vehicle")) { + return AirSimSettings::kSimModeTypeMultirotor; + } + else + return AirSimSettings::kSimModeTypeCar; +} + +FString ASimWorldGameMode::GetLaunchPath(const std::string& filename) +{ + FString launch_rel_path = FPaths::LaunchDir(); + FString abs_path = FPaths::ConvertRelativePathToFull(launch_rel_path); + return FPaths::Combine(abs_path, FString(filename.c_str())); +} + +bool ASimWorldGameMode::GetSettingsText(std::string& settingsText) +{ + return (GetSettingsTextFromCommandLine(settingsText) || + ReadSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || + // Use UE native module directory API to avoid encoding conversion and ensureFolder mkdir side effects + ReadSettingsTextFromFile(FPaths::Combine(FPlatformProcess::GetModulesDirectory(), TEXT("settings.json")), settingsText) || + ReadSettingsTextFromFile(GetLaunchPath("settings.json"), settingsText) || + ReadSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); +} + +bool ASimWorldGameMode::GetSettingsTextFromCommandLine(std::string& settingsText) +{ + bool found = false; + FString settingsTextFString; + const TCHAR* commandLineArgs = FCommandLine::Get(); + + if (FParse::Param(commandLineArgs, TEXT("-settings"))) { + FString commandLineArgsFString = FString(commandLineArgs); + int idx = commandLineArgsFString.Find(TEXT("-settings")); + FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (ReadSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { + return true; + } + + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { + settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); + found = true; + } + } + + return found; +} + +bool ASimWorldGameMode::ReadSettingsTextFromFile(const FString& settingsFilepath, std::string& settingsText) +{ + bool found = FPaths::FileExists(settingsFilepath); + if (found) { + FString settingsTextFStr; + bool readSuccessful = FFileHelper::LoadFileToString(settingsTextFStr, *settingsFilepath); + if (readSuccessful) { + UAirBlueprintLib::LogMessageString("Loaded settings from ", + TCHAR_TO_UTF8(*settingsFilepath), + LogDebugLevel::Informational); + settingsText = TCHAR_TO_UTF8(*settingsTextFStr); + } + else { + UAirBlueprintLib::LogMessageString("Cannot read file ", + TCHAR_TO_UTF8(*settingsFilepath), + LogDebugLevel::Failure); + throw std::runtime_error("Cannot read settings file."); + } + } + return found; +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp new file mode 100644 index 0000000000..75309e909d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp @@ -0,0 +1,129 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovFluidParams.h" + +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" +#include "Misc/FileHelper.h" +#include "Serialization/JsonReader.h" +#include "Serialization/JsonSerializer.h" + +namespace +{ +struct FShapeParams +{ + float Cd; + float AddedMassCoeff; // 相对 ρ·V 的附加质量系数 +}; + +FShapeParams ShapeTable(ERovHullShape Shape) +{ + switch (Shape) { + case ERovHullShape::Sphere: + return { 0.47f, 0.50f }; + case ERovHullShape::Box: + return { 1.05f, 0.70f }; + case ERovHullShape::Cylinder: + return { 0.82f, 0.60f }; + case ERovHullShape::Streamlined: + return { 0.08f, 0.10f }; + default: + return { 0.50f, 0.50f }; + } +} +} + +FRovFluidEstimate URovFluidLibrary::EstimateFluidParams(ERovHullShape Shape, FVector Dims, float Density) +{ + FRovFluidEstimate Out; + const FShapeParams P = ShapeTable(Shape); + Out.Cd = P.Cd; + + const float X = FMath::Max(Dims.X, KINDA_SMALL_NUMBER); + const float Y = FMath::Max(Dims.Y, KINDA_SMALL_NUMBER); + const float Z = FMath::Max(Dims.Z, KINDA_SMALL_NUMBER); + + switch (Shape) { + case ERovHullShape::Sphere: + Out.Volume = 4.0f / 3.0f * PI * X * X * X; + Out.RefArea = PI * X * X; + break; + case ERovHullShape::Box: + Out.Volume = 8.0f * X * Y * Z; + Out.RefArea = 4.0f * Y * Z; // 沿 X 迎流 + break; + case ERovHullShape::Cylinder: + Out.Volume = PI * X * X * 2.0f * Y; + Out.RefArea = PI * X * X; + break; + case ERovHullShape::Streamlined: + Out.Volume = 4.0f / 3.0f * PI * X * Y * Z; + Out.RefArea = PI * Y * Z; // 沿长轴 X 迎流 + break; + default: + break; + } + + Out.AddedMass = P.AddedMassCoeff * Density * Out.Volume; + return Out; +} + +float URovFluidLibrary::EstimateDragForce(ERovHullShape Shape, FVector Dims, float Speed, float Density) +{ + const FRovFluidEstimate E = EstimateFluidParams(Shape, Dims, Density); + return 0.5f * Density * E.Cd * E.RefArea * Speed * Speed; +} + +bool RovConfig::LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError) +{ + OutValues.Reset(); + FString Raw; + if (!FFileHelper::LoadFileToString(Raw, *Path)) { + OutError = FString::Printf(TEXT("读取失败:%s"), *Path); + return false; + } + + TSharedPtr Root; + const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) { + OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); + return false; + } + + for (const TPair>& Pair : Root->Values) { + if (!Pair.Value.IsValid() || Pair.Value->Type != EJson::Number) { + continue; + } + OutValues.Add(Pair.Key, static_cast(Pair.Value->AsNumber())); + } + return true; +} + +bool RovConfig::LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError) +{ + OutValues.Reset(); + FString Raw; + if (!FFileHelper::LoadFileToString(Raw, *Path)) { + OutError = FString::Printf(TEXT("读取失败:%s"), *Path); + return false; + } + + TSharedPtr Root; + const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) { + OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); + return false; + } + + const TArray>* Arr = nullptr; + if (!Root->TryGetArrayField(Field, Arr) || !Arr) { + OutError = FString::Printf(TEXT("缺少数组字段 %s"), *Field); + return false; + } + for (const TSharedPtr& V : *Arr) { + if (V.IsValid() && V->Type == EJson::Number) { + OutValues.Add(static_cast(V->AsNumber())); + } + } + return true; +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h new file mode 100644 index 0000000000..116fae1a77 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h @@ -0,0 +1,79 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 流体参数工具:外形 → 阻力系数估算、配置文件读取。 + +#pragma once + +#include "CoreMinimal.h" +#include "Kismet/BlueprintFunctionLibrary.h" +#include "RovFluidParams.generated.h" + +/** 典型外形。 */ +UENUM(BlueprintType) +enum class ERovHullShape : uint8 +{ + Sphere UMETA(DisplayName = "球体"), + Box UMETA(DisplayName = "立方体/长方体"), + Cylinder UMETA(DisplayName = "圆柱"), + Streamlined UMETA(DisplayName = "流线型"), +}; + +/** 由外形估算出的流体受力参数。 */ +USTRUCT(BlueprintType) +struct FRovFluidEstimate +{ + GENERATED_BODY() + + /** 迎流阻力系数 Cd。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float Cd = 0.5f; + + /** 迎流参考面积(m²)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float RefArea = 0.0f; + + /** 附加质量(kg)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float AddedMass = 0.0f; + + /** 排水体积(m³)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float Volume = 0.0f; +}; + +/** + * 外形到流体参数的估算。 + * + * 尺寸约定(单位 m,均为半尺寸): + * 球体 X = 半径 + * 立方体 X/Y/Z = 三个方向的半边长 + * 圆柱 X = 半径,Y = 半长(轴向迎流) + * 流线型 X/Y/Z = 椭球三个半轴,X 为长轴(长轴迎流) + */ +UCLASS() +class AIRSIM_API URovFluidLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + +public: + /** 由外形与尺寸估算 Cd、迎流面积、附加质量。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Fluid") + static FRovFluidEstimate EstimateFluidParams(ERovHullShape Shape, FVector Dims, float Density = 1000.0f); + + /** 按估算的 Cd 计算匀速迎流阻力 F = 0.5·ρ·Cd·A·v²(N)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Fluid") + static float EstimateDragForce(ERovHullShape Shape, FVector Dims, float Speed, float Density = 1000.0f); +}; + +/** 配置文件读取。 */ +namespace RovConfig +{ +/** + * 读取扁平 JSON 配置文件(键为字符串,值为数值)。 + * 数组与嵌套对象会被忽略,由调用方单独解析。 + */ +bool LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError); + +/** 读取配置文件里的数值数组字段(如 "Obstacles" 之外的一维数组)。 */ +bool LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError); +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp new file mode 100644 index 0000000000..127fd6f1f2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -0,0 +1,1926 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// 实现见头文件注释。 + +#include "Vehicles/Rov/RovMuJoCoBridge.h" + +#include "Engine/World.h" +#include "GameFramework/Actor.h" +#include "GameFramework/PlayerController.h" +#include "Components/PrimitiveComponent.h" +#include "Kismet/GameplayStatics.h" +#include "InputCoreTypes.h" +#include "HAL/FileManager.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.h" + +// URLab:MuJoCo 物理 +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Bodies/MjBody.h" + +DEFINE_LOG_CATEGORY_STATIC(LogRovBridge, Log, All); + +namespace +{ +/** 用 MuJoCo 四元数 (w,x,y,z) 旋转向量:v' = v + 2*(w*c + q×c),c = q×v */ +void RotByQuat(double qw, double qx, double qy, double qz, + double vx, double vy, double vz, + double& ox, double& oy, double& oz) +{ + const double cx = qy * vz - qz * vy; + const double cy = qz * vx - qx * vz; + const double cz = qx * vy - qy * vx; + const double dx = qy * cz - qz * cy; + const double dy = qz * cx - qx * cz; + const double dz = qx * cy - qy * cx; + ox = vx + 2.0 * (qw * cx + dx); + oy = vy + 2.0 * (qw * cy + dy); + oz = vz + 2.0 * (qw * cz + dz); +} +} + +std::function URovMuJoCoBridge::ExternalPoseProvider; + +URovMuJoCoBridge::URovMuJoCoBridge() +{ + // 需要 tick:键盘推进要每帧把力写进 mjData::xfrc_applied + PrimaryComponentTick.bCanEverTick = true; + PrimaryComponentTick.bStartWithTickEnabled = true; +} + +void URovMuJoCoBridge::BeginPlay() +{ + Super::BeginPlay(); + + if (!bEnableBridge) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已禁用(bEnableBridge=false),AirSim 保持原生物理")); + return; + } + + // 先尝试解析一次。注意:此时 MuJoCo 模型很可能还没编译完成, + // GetBodies() 会是空的——没关系,ProvidePose 里会惰性重试。 + ResolveTargets(); + + if (bDisablePawnPhysics) { + DisablePawnPhysics(); + } + + // 无论是否解析成功都注册 provider: + // 模型就绪后 ProvidePose 会自动接上,不需要重启 Pawn。 + ExternalPoseProvider = [this](FVector& OutPos, FQuat& OutRot) -> bool { + return ProvidePose(OutPos, OutRot); + }; + + bBridgeActive = true; + + if (TargetBody.IsValid()) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] ✅ 桥接已启用,跟随 body '%s'(Articulation='%s')"), *GetResolvedBodyName(), Articulation.IsValid() ? *Articulation->GetName() : TEXT("")); + } + else { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 位姿源已注册,但 MuJoCo 模型尚未就绪" + "(body 后缀 '%s')——将在运行中自动重试"), + *TargetBodyNameSuffix); + } +} + +void URovMuJoCoBridge::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + if (ContactLogArchive) { + ContactLogArchive->Close(); + delete ContactLogArchive; + ContactLogArchive = nullptr; + } + if (bBridgeActive) { + ExternalPoseProvider = nullptr; + bBridgeActive = false; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 桥接已解除")); + } + Super::EndPlay(EndPlayReason); +} + +void URovMuJoCoBridge::ResolveTargets() const +{ + UWorld* World = GetWorld(); + if (!World) return; + + Articulation = Cast( + UGameplayStatics::GetActorOfClass(World, AMjArticulation::StaticClass())); + + if (!Articulation.IsValid()) { + return; // 场景里还没有 articulation,静默等待 + } + + // URLab 会给 spec 元素名加蓝图前缀,所以用后缀匹配 + TArray Bodies = Articulation->GetBodies(); + for (UMjBody* Body : Bodies) { + if (!Body) continue; + const FString Name = Body->GetName(); + if (Name.EndsWith(TargetBodyNameSuffix, ESearchCase::IgnoreCase)) { + TargetBody = Body; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 匹配到 body: '%s'(后缀 '%s')"), *Name, *TargetBodyNameSuffix); + return; + } + } + + // 有 articulation 但没匹配到 body:只在第一次打印候选,避免刷屏 + if (!bResolveWarned && Bodies.Num() > 0) { + bResolveWarned = true; + UE_LOG(LogRovBridge, Warning, TEXT("[RovMuJoCoBridge] 没有 body 以 '%s' 结尾。候选(%d 个):"), *TargetBodyNameSuffix, Bodies.Num()); + for (UMjBody* Body : Bodies) { + if (Body) { + UE_LOG(LogRovBridge, Warning, TEXT(" %s"), *Body->GetName()); + } + } + } +} + +bool URovMuJoCoBridge::ProvidePose(FVector& OutPosition, FQuat& OutRotation) const +{ + // 惰性解析:MuJoCo 模型通常在 Pawn 生成之后才编译完成, + // 所以这里每帧检查一次,一旦 body 出现就自动接上。 + if (!TargetBody.IsValid()) { + ResolveTargets(); + } + + if (!TargetBody.IsValid()) { + return false; + } + + // 用绑定状态判断「是否就绪」,而不是靠位姿是否为零—— + // body 真的待在原点时,零位姿是合法值,不能当成未就绪。 + if (!TargetBody->IsBound()) { + return false; + } + + // 注意:这两个函数直接读 MuJoCo 的 xpos / xquat 数组, + // 不依赖 UMjBody::TickComponent 是否已经执行,因此不存在「差一帧」问题。 + OutPosition = TargetBody->GetWorldPosition() + WorldOffset; + OutRotation = TargetBody->GetWorldRotation(); + + // 一次性物理诊断:确认 MuJoCo 到底有没有在步进。 + // 如果这里 IsRunning=0 或仿真时间一直是 0,说明物理循环没跑起来。 + static bool bLoggedPhysicsDiag = false; + if (!bLoggedPhysicsDiag) { + bLoggedPhysicsDiag = true; + if (AAMjManager* Mgr = AAMjManager::GetManager()) { + const double Timestep = (Mgr->PhysicsEngine && Mgr->PhysicsEngine->m_model) + ? Mgr->PhysicsEngine->m_model->opt.timestep + : -1.0; + + // UMjPhysicsEngine 的 bIsPaused 默认是 true(URLab 让用户在 + // MjSimulate 控件里手动点播放)。Pawn 要跟随就必须让仿真跑起来。 + if (bAutoStartSimulation && !Mgr->IsRunning()) { + Mgr->SetPaused(false); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] MuJoCo 仿真原本处于暂停状态,已自动启动")); + } + + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 物理诊断: IsRunning=%d IsInitialized=%d " + "SimTime=%.4fs timestep=%.4fs SimSpeed=%.0f%%"), + Mgr->IsRunning() ? 1 : 0, + Mgr->IsInitialized() ? 1 : 0, + Mgr->GetSimTime(), + Timestep, + Mgr->PhysicsEngine ? Mgr->PhysicsEngine->SimSpeedPercent : -1.0f); + } + } + + // 节流调试日志:每 60 次调用打印一次 MuJoCo 位姿 vs Pawn 实际位姿, + // 用来确认「Pawn 真的跟着 MuJoCo 走」。设 LogRovBridge 为 Log 级别即可看到。 + static int32 CallCounter = 0; + if (++CallCounter % 60 == 0) { + const AActor* Owner = GetOwner(); + const FVector PawnPos = Owner ? Owner->GetActorLocation() : FVector::ZeroVector; + AAMjManager* Mgr = AAMjManager::GetManager(); + const float SimTime = Mgr ? Mgr->GetSimTime() : -1.0f; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 跟随中 #%d 仿真时间=%.3fs MuJoCo=(%s) Pawn=(%s) 差值=%.2f cm"), CallCounter, SimTime, *OutPosition.ToString(), *PawnPos.ToString(), FVector::Dist(OutPosition, PawnPos)); + } + + return true; +} + +void URovMuJoCoBridge::DisablePawnPhysics() +{ + AActor* Owner = GetOwner(); + if (!Owner) return; + + int32 DisabledPrimitives = 0; + + TArray Primitives; + Owner->GetComponents(Primitives); + + for (UPrimitiveComponent* Prim : Primitives) { + if (!Prim) continue; + + if (Prim->IsSimulatingPhysics()) { + Prim->SetSimulatePhysics(false); + } + // 关掉碰撞:MuJoCo 已经负责碰撞,UE 侧再算一次会双计。 + if (Prim->GetCollisionEnabled() != ECollisionEnabled::NoCollision) { + Prim->SetCollisionEnabled(ECollisionEnabled::NoCollision); + ++DisabledPrimitives; + } + } + + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已关闭 Pawn '%s' 的物理与碰撞(%d 个图元)," + "避免与 MuJoCo 双计"), + *Owner->GetName(), + DisabledPrimitives); +} + +FString URovMuJoCoBridge::GetResolvedBodyName() const +{ + return TargetBody.IsValid() ? TargetBody->GetName() : FString(); +} + +// 键盘推进 +// +// 推力通过 MuJoCo 的 xfrc_applied 施加:桥接关闭了 ARovPawn 的物理与碰撞, +// AirSim 的 RC 通道没有物理体承接;模型里的 actuator 只有机械臂关节。 +// xfrc_applied 不会被 mj_step 自动清零,每帧需先清再写。 +// 力的量级按质量缩放,换不同质量的 ROV 手感一致。 + +void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + + if (bBridgeActive && bEnableKeyboardControl) { + ApplyControlForce(); + } +} + +void URovMuJoCoBridge::ApplyConfigFile() +{ + if (!bLoadConfigFile) { + UE_LOG(LogRovBridge, Log, TEXT("[配置] bLoadConfigFile=false,跳过配置文件")); + return; + } + + FString Path; + if (!FParse::Value(FCommandLine::Get(), TEXT("RovConfig="), Path)) { + Path = FPaths::Combine(FPaths::ProjectConfigDir(), TEXT("RovParams.json")); + } + Path = FPaths::ConvertRelativePathToFull(Path); + if (!FPaths::FileExists(Path)) { + UE_LOG(LogRovBridge, Warning, TEXT("[配置] 未找到配置文件 %s,使用类默认值 + 命令行覆盖"), *Path); + return; + } + + TMap Kv; + FString Err; + if (!RovConfig::LoadKeyValues(Path, Kv, Err)) { + UE_LOG(LogRovBridge, Warning, TEXT("[配置] %s"), *Err); + return; + } + + auto ApplyF = [&Kv](const TCHAR* Key, float& Target) { + if (const float* V = Kv.Find(Key)) { + Target = *V; + } + }; + auto ApplyB = [&Kv](const TCHAR* Key, bool& Target) { + if (const float* V = Kv.Find(Key)) { + Target = (*V) != 0.0f; + } + }; + auto ApplyV = [&Kv](const TCHAR* Prefix, FVector& Target) { + const FString P(Prefix); + if (const float* X = Kv.Find(P + TEXT("X"))) { + Target.X = *X; + } + if (const float* Y = Kv.Find(P + TEXT("Y"))) { + Target.Y = *Y; + } + if (const float* Z = Kv.Find(P + TEXT("Z"))) { + Target.Z = *Z; + } + }; + + ApplyF(TEXT("FluidDensity"), FluidDensity); + ApplyF(TEXT("FluidViscosity"), FluidViscosity); + ApplyF(TEXT("DisplacedVolume"), DisplacedVolume); + ApplyF(TEXT("BuoyancyScale"), BuoyancyScale); + ApplyF(TEXT("BuoyancyArm"), BuoyancyArm); + ApplyF(TEXT("ArmBuoyancyVolume"), ArmBuoyancyVolume); + ApplyF(TEXT("ArmBuoyancyScale"), ArmBuoyancyScale); + ApplyF(TEXT("ArmJointDamping"), ArmJointDamping); + ApplyF(TEXT("ArmJointStiffness"), ArmJointStiffness); + ApplyB(TEXT("ArmGravityComp"), bArmGravityComp); + ApplyF(TEXT("ThrusterMaxRatio"), ThrusterMaxRatio); + ApplyF(TEXT("WaterSurfaceZ"), WaterSurfaceZ); + ApplyF(TEXT("AngularDofDamping"), AngularDofDamping); + ApplyF(TEXT("AngularDampingCoeff"), AngularDampingCoeff); + ApplyF(TEXT("AttitudeBandwidth"), AttitudeBandwidth); + ApplyF(TEXT("AttitudeZeta"), AttitudeZeta); + ApplyF(TEXT("AttitudeIntegralGain"), AttitudeIntegralGain); + ApplyF(TEXT("LevelBandwidth"), LevelBandwidth); + ApplyF(TEXT("LevelZeta"), LevelZeta); + ApplyF(TEXT("YawBandwidth"), YawBandwidth); + ApplyF(TEXT("YawZeta"), YawZeta); + ApplyF(TEXT("YawRateDegPerSec"), YawRateDegPerSec); + ApplyF(TEXT("CurrentDragCoeff"), CurrentDragCoeff); + ApplyF(TEXT("OceanCurrentMeanSpeed"), OceanCurrentMeanSpeed); + ApplyB(TEXT("EnableAttitudeHold"), bEnableAttitudeHold); + ApplyB(TEXT("EnableDepthHold"), bEnableDepthHold); + ApplyB(TEXT("EnableOceanCurrent"), bEnableOceanCurrent); + ApplyB(TEXT("UseNativeFluid"), bUseNativeFluid); + + ApplyB(TEXT("UseExplicitDrag"), bUseExplicitDrag); + ApplyV(TEXT("DragCoeff"), DragCoeff); + ApplyV(TEXT("DragRefArea"), DragRefArea); + ApplyB(TEXT("UseAddedMass"), bUseAddedMass); + ApplyV(TEXT("AddedMass"), AddedMass); + + ApplyB(TEXT("AutoEstimateFluidParams"), bAutoEstimateFluidParams); + ApplyV(TEXT("HullDims"), HullDims); + if (const float* S = Kv.Find(TEXT("HullShape"))) { + HullShape = static_cast(FMath::Clamp(FMath::RoundToInt(*S), 0, 3)); + } + if (bAutoEstimateFluidParams) { + const FRovFluidEstimate E = URovFluidLibrary::EstimateFluidParams(HullShape, HullDims, FluidDensity); + DragCoeff = FVector(E.Cd, E.Cd, E.Cd); + DragRefArea = FVector(E.RefArea, E.RefArea, E.RefArea); + AddedMass = FVector(E.AddedMass, E.AddedMass, E.AddedMass); + bUseExplicitDrag = true; + bUseAddedMass = true; + UE_LOG(LogRovBridge, Log, TEXT("[配置] 外形估算:Cd=%.2f 迎流面积=%.4f m² 附加质量=%.1f kg 排水体积=%.4f m³"), E.Cd, E.RefArea, E.AddedMass, E.Volume); + } + + ApplyB(TEXT("LogContacts"), bLogContacts); + if (const float* V = Kv.Find(TEXT("ContactLogInterval"))) { + ContactLogInterval = FMath::Max(1, FMath::RoundToInt(*V)); + } + + LoadedConfigPath = Path; + UE_LOG(LogRovBridge, Log, TEXT("[配置] 已加载 %s,共 %d 个数值项"), *Path, Kv.Num()); +} + +void URovMuJoCoBridge::LogContacts(mjModel_* Model, mjData_* Data) +{ + if (!bLogContacts || !Model || !Data) { + return; + } + if (++ContactLogStep % FMath::Max(1, ContactLogInterval) != 0) { + return; + } + + if (!ContactLogArchive) { + FString Path = ContactLogPath.IsEmpty() + ? FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("RovContactLog.csv")) + : ContactLogPath; + Path = FPaths::ConvertRelativePathToFull(Path); + ContactLogArchive = IFileManager::Get().CreateFileWriter(*Path, FILEWRITE_Append); + if (!ContactLogArchive) { + UE_LOG(LogRovBridge, Warning, TEXT("[接触日志] 无法写入 %s"), *Path); + bLogContacts = false; + return; + } + UE_LOG(LogRovBridge, Log, TEXT("[接触日志] 写入 %s(每 %d 个物理步一条)"), *Path, ContactLogInterval); + } + + auto WriteLine = [this](const FString& Line) { + FTCHARToUTF8 Utf8(*Line); + ContactLogArchive->Serialize(static_cast(const_cast(Utf8.Get())), Utf8.Length()); + }; + + if (!bContactLogHeaderWritten) { + WriteLine(TEXT("time_s,body1,body2,normal_N,pos_x,pos_y,pos_z\n")); + bContactLogHeaderWritten = true; + } + + for (int32 i = 0; i < Data->ncon; ++i) { + const mjContact& C = Data->contact[i]; + mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; + mj_contactForce(Model, Data, i, F6); + + const int32 B1 = Model->geom_bodyid[C.geom1]; + const int32 B2 = Model->geom_bodyid[C.geom2]; + const FString N1 = (B1 >= 0 && Model->name_bodyadr[B1] >= 0) + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B1])) + : FString(TEXT("world")); + const FString N2 = (B2 >= 0 && Model->name_bodyadr[B2] >= 0) + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B2])) + : FString(TEXT("world")); + + WriteLine(FString::Printf(TEXT("%.5f,%s,%s,%.4f,%.5f,%.5f,%.5f\n"), + Data->time, + *N1, + *N2, + F6[0], + C.pos[0], + C.pos[1], + C.pos[2])); + } + + // 立即落盘,避免进程被强杀时缓冲区内容丢失 + ContactLogArchive->Flush(); +} + +void URovMuJoCoBridge::GetStatusSnapshot(FRovStatusSnapshot& Out) const +{ + Out.RollDeg = (float)CachedRollDeg.load(); + Out.PitchDeg = (float)CachedPitchDeg.load(); + Out.YawDeg = (float)CachedYawDeg.load(); + Out.DepthM = (float)CachedDepthM.load(); + Out.ThrustN = (float)CachedThrustN.load(); + Out.ContactForceN = (float)CachedContactForce.load(); + Out.VelocityMps = FVector((float)CachedVelocityX.load(), (float)CachedVelocityY.load(), (float)CachedVelocityZ.load()); + Out.ContactPoint = FVector((float)CachedContactPointX.load(), (float)CachedContactPointY.load(), (float)CachedContactPointZ.load()); + Out.ContactForceDir = FVector((float)CachedContactDirX.load(), (float)CachedContactDirY.load(), (float)CachedContactDirZ.load()); + Out.FlowVelocityMps = FVector((float)CachedFlowX.load(), (float)CachedFlowY.load(), (float)CachedFlowZ.load()); + Out.bValid = bBridgeActive && TargetBody.IsValid(); +} + +void URovMuJoCoBridge::ApplyControlForce() +{ + if (!TargetBody.IsValid() || !TargetBody->IsBound()) { + return; + } + + AAMjManager* Mgr = AAMjManager::GetManager(); + if (!Mgr || !Mgr->PhysicsEngine) { + return; + } + + mjModel* Model = Mgr->PhysicsEngine->GetModel(); + mjData* Data = Mgr->PhysicsEngine->GetData(); + if (!Model || !Data) { + return; + } + + const int32 BodyId = TargetBody->GetMjID(); + if (BodyId < 0 || BodyId >= Model->nbody) { + return; + } + + const double Gravity = FMath::Abs((double)Model->opt.gravity[2]); + + // ---------------- 一次性初始化:打开原生流体模型 + 受力诊断 ---------------- + static bool bFluidInitialized = false; + if (!bFluidInitialized) { + bFluidInitialized = true; + + // 命令行覆盖,便于现场调参(不用重编) + auto FloatParam = [](const TCHAR* Key, float Default) -> float { + float V = Default; + return FParse::Value(FCommandLine::Get(), Key, V) ? V : Default; + }; + // 配置文件先加载,命令行随后覆盖,保证「命令行 > 配置文件 > 默认值」 + ApplyConfigFile(); + DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); + BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); + BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); + ArmBuoyancyVolume = FloatParam(TEXT("RovArmBuoyVolume="), ArmBuoyancyVolume); + ArmBuoyancyScale = FloatParam(TEXT("RovArmBuoyScale="), ArmBuoyancyScale); + AngularDampingCoeff = FloatParam(TEXT("RovAngularDamping="), AngularDampingCoeff); + ArmJointDamping = FloatParam(TEXT("RovArmDamping="), ArmJointDamping); + ThrusterMaxRatio = FloatParam(TEXT("RovThrustRatio="), ThrusterMaxRatio); + WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); + if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { + bEnableAttitudeHold = false; + } + if (FParse::Param(FCommandLine::Get(), TEXT("RovContactTest"))) { + ContactTestForce = 900.0; + bLogContacts = true; + bEnableDepthHold = false; + UE_LOG(LogRovBridge, Log, TEXT("[接触试验] 施加 %.0f N 下压力,用于产生海床接触"), ContactTestForce); + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 臂排水=%.2f L×%.2f " + "推力比=%.2f 水面Z=%.2f m"), + DisplacedVolume, + BuoyancyScale, + ArmBuoyancyVolume, + ArmBuoyancyScale, + ThrusterMaxRatio, + WaterSurfaceZ); + + if (bUseNativeFluid) { + Model->opt.density = FluidDensity; + Model->opt.viscosity = FluidViscosity; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][流体] 已启用 MuJoCo 椭球流体模型 density=%.1f kg/m³ viscosity=%.6f Pa·s"), Model->opt.density, Model->opt.viscosity); + } + else { + Model->opt.density = 0.0; + Model->opt.viscosity = 0.0; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][流体] 已关闭(density=viscosity=0)")); + } + + // 质量诊断:确认 UE 里这个模型到底多重 + const double TotalMass = Model->body_subtreemass[BodyId]; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][质量] 目标 body id=%d body_mass=%.3f kg body_subtreemass=%.3f kg nbody=%d"), BodyId, Model->body_mass[BodyId], TotalMass, Model->nbody); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][惯量] body_inertia=(%.5f, %.5f, %.5f) kg·m² " + "(角阻尼 Kd = %.1f × 平均惯量 %.5f = %.3f N·m·s/rad)"), + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + AngularDampingCoeff, + (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0, + AngularDampingCoeff * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0); + for (int32 i = 1; i < Model->nbody; ++i) { + if (Model->body_mass[i] > 0.5) { + const char* Name = (Model->names && Model->name_bodyadr[i] >= 0) + ? (Model->names + Model->name_bodyadr[i]) + : "?"; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][质量] body[%2d] %-24s mass=%.3f subtreemass=%.3f"), i, ANSI_TO_TCHAR(Name), Model->body_mass[i], Model->body_subtreemass[i]); + } + } + + // 机械臂关节阻尼 + 刚度:模型的机械臂关节没有 ctrl 输入, + // 会自由摆动,反作用力矩把 ROV 本体晃得不停翻滚。 + // 阻尼抵抗运动、刚度把它锁在参考角度,两者合起来相当于位置伺服。 + { + int32 DampedDofs = 0; + int32 StiffJoints = 0; + for (int32 j = 0; j < Model->njnt; ++j) { + if (Model->jnt_type[j] == mjJNT_FREE) { + continue; + } + const int32 Adr = Model->jnt_dofadr[j]; + const int32 Num = (Model->jnt_type[j] == mjJNT_BALL) ? 3 : 1; + for (int32 k = 0; k < Num; ++k) { + Model->dof_damping[Adr + k] = ArmJointDamping; + ++DampedDofs; + } + Model->jnt_stiffness[j] = (mjtNum)ArmJointStiffness; + ++StiffJoints; + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][机械臂] %d 个关节:阻尼 %.1f N·m·s/rad、刚度 %.1f N·m/rad(%d 个自由度)"), StiffJoints, ArmJointDamping, ArmJointStiffness, DampedDofs); + } + + // 机械臂重力补偿:抵消其自重,消除对艇体的恒定倾覆力矩 + if (bArmGravityComp) { + int32 CompBodies = 0; + for (int32 b = 1; b < Model->nbody; ++b) { + if (b == BodyId) { + continue; // ROV 本体不补偿 + } + Model->body_gravcomp[b] = 1.0; + ++CompBodies; + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][机械臂] 已对 %d 个 body 做重力补偿(gravcomp=1.0)"), CompBodies); + } + + // 复合惯量:先用本体惯量兜底,等仿真真正跑起来再换成 6×6 质量阵的值。 + // (qM 只有在 mj_forward/mj_step 之后才有效,首帧读会拿到垃圾值; + // 打包版首帧时序和编辑器不同,曾读到极小值 → 控制器力矩≈0 → ROV 缓慢翻转。) + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Model->body_inertia[3 * BodyId + a]; + } + + // 洋流模型初始化(三层级联) + if (FParse::Param(FCommandLine::Get(), TEXT("RovOceanCurrent"))) { + bEnableOceanCurrent = true; // 命令行强制开启,便于测试 + } + if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { + bEnableAttitudeHold = false; // 自检时关掉姿态控制器,避免干扰 + } + OceanCurrent.bEnabled = bEnableOceanCurrent; + OceanCurrent.MeanSpeed = OceanCurrentMeanSpeed; + OceanCurrent.Seed = 42; + OceanCurrent.Initialize(); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][洋流] %s 平均流速=%.2f m/s 分层=%d 层 湍流强度=%.2f"), bEnableOceanCurrent ? TEXT("已启用") : TEXT("已关闭"), OceanCurrentMeanSpeed, OceanCurrent.Layers.Num(), OceanCurrent.TurbIntensity); + + // 单位诊断:确认 MuJoCo 模型到底是米还是厘米 + { + int32 RovGeomId = -1; + for (int32 g = 0; g < Model->ngeom; ++g) { + const char* GName = (Model->names && Model->name_geomadr[g] >= 0) + ? (Model->names + Model->name_geomadr[g]) + : ""; + if (FString(ANSI_TO_TCHAR(GName)).EndsWith(TEXT("rov_body"))) { + RovGeomId = g; + break; + } + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] gravity=%.3f timestep=%.5f stat.extent=%.3f " + "模型包围盒中心=(%.2f,%.2f,%.2f)"), + Model->opt.gravity[2], + Model->opt.timestep, + Model->stat.extent, + Model->stat.center[0], + Model->stat.center[1], + Model->stat.center[2]); + if (RovGeomId >= 0) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] rov_body geom size=(%.4f, %.4f, %.4f) " + "(若≈0.4/0.3/0.2 则模型是米;若≈40/30/20 则是厘米)"), + Model->geom_size[3 * RovGeomId + 0], + Model->geom_size[3 * RovGeomId + 1], + Model->geom_size[3 * RovGeomId + 2]); + } + if (AActor* Owner = GetOwner()) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] Pawn 世界坐标=(%.2f, %.2f, %.2f) cm " + "MuJoCo xpos=(%.2f, %.2f, %.2f)"), + Owner->GetActorLocation().X, + Owner->GetActorLocation().Y, + Owner->GetActorLocation().Z, + Data->xpos[3 * BodyId + 0], + Data->xpos[3 * BodyId + 1], + Data->xpos[3 * BodyId + 2]); + } + } + + // 受力诊断:三个数摆出来,一眼能看出浮沉 + const double Weight = TotalMass * Gravity; + const double Buoyancy = FluidDensity * Gravity * DisplacedVolume * BuoyancyScale; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][受力] 重力=%.1f N 浮力=%.1f N 净力=%+.1f N(正=上浮,负=下沉) " + "排水体积=%.4f m³ 浮力倍率=%.2f"), + Weight, + Buoyancy, + Buoyancy - Weight, + DisplacedVolume, + BuoyancyScale); + } + + // 复合惯量延迟计算:qM 只在 mj_forward/mj_step 之后有效, + // 因此等仿真时间过 0.2 s 再取,并对量级做下限校验。 + { + static bool bEffInertiaDone = false; + if (!bEffInertiaDone && Data->time > 0.2 && Data->qM) { + bEffInertiaDone = true; + const int32 Nv = Model->nv; + bool bOk = (Nv >= 6); + double Vals[3] = { 0.0, 0.0, 0.0 }; + if (bOk) { + TArray M; + M.SetNumZeroed(Nv * Nv); + mj_fullM(Model, M.GetData(), Data->qM); + for (int32 a = 0; a < 3; ++a) { + const double V = M[(3 + a) * Nv + (3 + a)]; + // 这台 ROV 的转动惯量量级是几十 kg·m²,小于 1 一律视为无效 + if (!FMath::IsFinite(V) || V < 1.0) { + bOk = false; + break; + } + Vals[a] = V; + } + } + if (bOk) { + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Vals[a]; + } + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][复合惯量] 延迟计算 t=%.2f " + "EffInertia=(%.3f, %.3f, %.3f) 来源=%s"), + Data->time, + EffInertia[0], + EffInertia[1], + EffInertia[2], + bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); + } + } + + APlayerController* PC = GetWorld() ? GetWorld()->GetFirstPlayerController() : nullptr; + if (!PC) { + return; + } + + // 本体坐标系下的期望方向 + // 注意:A/D 的符号和直觉相反——这个模型的 Y 轴朝左,所以 A 用 +1、D 用 -1。 + // 垂直用 Q/E(PageUp/PageDown 在笔记本上常常没有,保留作别名)。 + double LocalX = 0.0, LocalY = 0.0, LocalZ = 0.0; + if (PC->IsInputKeyDown(EKeys::W)) { + LocalX += 1.0; + } + if (PC->IsInputKeyDown(EKeys::S)) { + LocalX -= 1.0; + } + if (PC->IsInputKeyDown(EKeys::A)) { + LocalY += 1.0; + } + if (PC->IsInputKeyDown(EKeys::D)) { + LocalY -= 1.0; + } + if (PC->IsInputKeyDown(EKeys::Q) || PC->IsInputKeyDown(EKeys::PageUp)) { + LocalZ += 1.0; + } + if (PC->IsInputKeyDown(EKeys::E) || PC->IsInputKeyDown(EKeys::PageDown)) { + LocalZ -= 1.0; + } + + // 按键诊断:状态一变就记一条(不节流),用来确认输入到底有没有进到游戏 + { + // 同时读两条通道:PlayerController 和 PlayerInput,任一为真即算按下。 + // 有些 build 里 IsInputKeyDown 取不到状态,PlayerInput 可以兜底。 + auto IsDown = [PC](const FKey& Key) -> bool { + bool b = PC->IsInputKeyDown(Key); + if (!b && PC->PlayerInput) { + b = PC->PlayerInput->IsPressed(Key); + } + return b; + }; + + static int32 PrevKeyMask = 0; + const int32 KeyMask = + (IsDown(EKeys::W) ? 1 : 0) | + (IsDown(EKeys::S) ? 2 : 0) | + (IsDown(EKeys::A) ? 4 : 0) | + (IsDown(EKeys::D) ? 8 : 0) | + (IsDown(EKeys::Q) ? 16 : 0) | + (IsDown(EKeys::E) ? 32 : 0) | + (IsDown(EKeys::Z) ? 64 : 0); + if (KeyMask != PrevKeyMask) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][按键] 状态 %d -> %d (W=1 S=2 A=4 D=8 Q=16 E=32 Z=64)"), PrevKeyMask, KeyMask); + PrevKeyMask = KeyMask; + } + + // 每秒打印一次焦点状态,确认游戏窗口到底有没有拿到输入焦点 + static int32 FocusLogTick = -1; + const double NowSec = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + const int32 CurTick = (int32)NowSec; + if (CurTick != FocusLogTick) { + FocusLogTick = CurTick; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][焦点] IsLocalController=%d PlayerInput=%s " + "bShowMouseCursor=%d bIgnoreInput=%d 按键掩码=%d"), + PC->IsLocalController() ? 1 : 0, + PC->PlayerInput ? TEXT("valid") : TEXT("NULL"), + PC->bShowMouseCursor ? 1 : 0, + PC->IsInputKeyDown(EKeys::LeftMouseButton) ? 1 : 0, + KeyMask); + } + } + + // 命令行自检:-RovThrustTest 会在启动后持续上浮 2 秒并打印 Z + static const bool bAutoThrustTest = + FParse::Param(FCommandLine::Get(), TEXT("RovThrustTest")); + static double AutoTestStartTime = -1.0; + static int32 AutoTestLastTick = -1; + if (bAutoThrustTest) { + UWorld* W = GetWorld(); + const double Now = W ? W->GetTimeSeconds() : 0.0; + if (AutoTestStartTime < 0.0) { + AutoTestStartTime = Now; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][自检] 开始:上浮 2 秒,起始 Z=%.3f"), Data->xpos[3 * BodyId + 2]); + } + if (Now - AutoTestStartTime < 10.0) { + LocalZ = 1.0; + } + const int32 Tick = (int32)((Now - AutoTestStartTime) * 2.0); + if (Tick != AutoTestLastTick) { + AutoTestLastTick = Tick; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][自检] t=%.1fs Z=%.3f 总Fz=%.1f N vz=%.3f"), Now - AutoTestStartTime, Data->xpos[3 * BodyId + 2], CachedForceZ.load(), Data->cvel[6 * BodyId + 5]); + } + } + + const double Mass = Model->body_subtreemass[BodyId]; + if (Mass <= 0.0) { + return; + } + + const mjtNum* Q = &Data->xquat[4 * BodyId]; // (w, x, y, z) + const double Qw = Q[0], Qx = Q[1], Qy = Q[2], Qz = Q[3]; + + // 只取偏航角,忽略俯仰和横滚 —— 否则 ROV 一翻滚推力方向就乱 + const double Yaw = FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), + 1.0 - 2.0 * (Qy * Qy + Qz * Qz)); + const double Cy = FMath::Cos(Yaw); + const double Sy = FMath::Sin(Yaw); + + // ------------------------------------------------------------------ + // 受力分解:每一项独立计算,最后相加。不直接设定合力。 + // ------------------------------------------------------------------ + + // 【1】推进器推力:键盘只给比例,力 = 比例 × 最大推力 + const double ThrusterMax = Mass * Gravity * ThrusterMaxRatio; + double ThrusterX = ThrusterMax * (LocalX * Cy - LocalY * Sy); + double ThrusterY = ThrusterMax * (LocalX * Sy + LocalY * Cy); + double ThrusterZ = ThrusterMax * LocalZ; + + const double RovZ = Data->xpos[3 * BodyId + 2]; + const FVector RovPos(Data->xpos[3 * BodyId + 0], + Data->xpos[3 * BodyId + 1], + Data->xpos[3 * BodyId + 2]); + + const double FrameDt = GetWorld() ? FMath::Max(GetWorld()->GetDeltaSeconds(), 1e-4f) : 0.016; + + // 【航向调整】← / → 连续改变目标航向(姿态保持开启时生效) + // + // 方向按画面定义:MuJoCo→UE 为镜像映射(Y 取反),MuJoCo 的 +yaw 在画面上是 -yaw, + // 因此 → = 画面右转 = MuJoCo yaw 减小。 + // 按住期间同时给出角速度前馈,避免目标航向跑得比机身快导致误差累积过 180°。 + double YawCmdRateDeg = 0.0; // 本帧航向角速度指令(deg/s,MuJoCo 符号) + if (bEnableAttitudeHold && PC) { + const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); + const bool bYawRight = PC->IsInputKeyDown(EKeys::Right); + if (bYawLeft != bYawRight) { + YawCmdRateDeg = bYawLeft ? +(double)YawRateDegPerSec + : -(double)YawRateDegPerSec; + } + } + + // 命令行自检:-RovYawTest 在 3~11 秒模拟"一直按住 →"(画面右转 = MuJoCo yaw 减小), + // 转过的总角度超过 360°,用来验证方向、以及转满一圈后不回弹。 + static const bool bYawTest = FParse::Param(FCommandLine::Get(), TEXT("RovYawTest")); + if (bYawTest && bEnableAttitudeHold && YawCmdRateDeg == 0.0) { + const double TestT = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (TestT > 3.0 && TestT < 11.0) { + YawCmdRateDeg = -(double)YawRateDegPerSec; + } + } + + if (YawCmdRateDeg != 0.0) { + TargetYawDeg += (float)(YawCmdRateDeg * FrameDt); + while (TargetYawDeg > 180.0f) { + TargetYawDeg -= 360.0f; + } + while (TargetYawDeg < -180.0f) { + TargetYawDeg += 360.0f; + } + static int32 YawLogCounter = 0; + if ((++YawLogCounter % 30) == 0) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f° 指令角速度 = %+.0f°/s(←/→)"), TargetYawDeg, YawCmdRateDeg); + } + } + + // 航向角速度前馈交给物理线程的姿态环 + CachedYawFFRadPerSec.store(FMath::DegreesToRadians(YawCmdRateDeg)); + + // 【2】洋流:三层级联求当前流速 + const FVector VCurrent = OceanCurrent.GetVelocity(RovPos, (float)FrameDt); + + // 【3】定深(PID):按 Z 键锁定当前高度,再按解除 + if (bEnableDepthHold && PC) { + const bool bDepthKeyDown = PC->IsInputKeyDown(EKeys::Z); + if (bDepthKeyDown && !bPrevDepthKeyDown) { + bDepthHoldActive = !bDepthHoldActive; + DepthPID.Reset(); + if (bDepthHoldActive) { + DepthHoldTargetZ = RovZ; + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深] %s(目标 Z=%.3f m)"), bDepthHoldActive ? TEXT("已锁定") : TEXT("已解除"), DepthHoldTargetZ); + } + bPrevDepthKeyDown = bDepthKeyDown; + } + + // 出生自动锁定深度:避免浮力略小于重力时一路沉到海底,保证演示时停在水面附近 + static bool bAutoDepthInit = false; + if (bEnableDepthHold && !bAutoDepthInit) { + bAutoDepthInit = true; + bDepthHoldActive = true; + DepthPID.Reset(); + DepthHoldTargetZ = RovZ; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深] 出生自动锁定 Z=%.3f m"), DepthHoldTargetZ); + } + + // 命令行自检:-RovDepthHoldTest 在 3 秒后自动锁定当前深度并打印误差, + // 用来验证 PID 定深,不依赖键盘。 + static const bool bDepthHoldTest = + FParse::Param(FCommandLine::Get(), TEXT("RovDepthHoldTest")); + static int32 DepthTestLastTick = -1; + if (bDepthHoldTest) { + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (Now > 1.0 && !bDepthHoldActive) { + bDepthHoldActive = true; + DepthPID.Reset(); + DepthHoldTargetZ = RovZ; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深自检] 锁定 Z=%.3f m"), DepthHoldTargetZ); + } + } + + // 命令行自检:-RovSpinTest 在 2 秒时给一个 3 rad/s 的三轴角速度脉冲, + // 然后打印角速度衰减,验证流体模型的旋转阻力(验收项 P7)。 + static const bool bSpinTest = FParse::Param(FCommandLine::Get(), TEXT("RovSpinTest")); + static bool bSpinApplied = false; + static int32 SpinLogTick = -1; + if (bSpinTest && Model->body_jntnum[BodyId] >= 1) { + const int32 SpinJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[SpinJnt] == mjJNT_FREE) { + const int32 SpinDof = Model->jnt_dofadr[SpinJnt]; + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (!bSpinApplied && Now > 2.0) { + bSpinApplied = true; + // 放到水中(Z=-5m)并清零线速度,避免海底接触和浮力突变污染测量。 + // 注意:姿态控制在自检期间会被跳过,否则闭环会把脉冲立刻压掉。 + const int32 QPosAdr = Model->jnt_qposadr[SpinJnt]; + Data->qpos[QPosAdr + 0] = 0.0; + Data->qpos[QPosAdr + 1] = 0.0; + Data->qpos[QPosAdr + 2] = -5.0; + Data->qpos[QPosAdr + 3] = 1.0; // quat w + Data->qpos[QPosAdr + 4] = 0.0; + Data->qpos[QPosAdr + 5] = 0.0; + Data->qpos[QPosAdr + 6] = 0.0; + Data->qvel[SpinDof + 0] = 0.0; + Data->qvel[SpinDof + 1] = 0.0; + Data->qvel[SpinDof + 2] = 0.0; + Data->qvel[SpinDof + 3] = 3.0; + Data->qvel[SpinDof + 4] = 3.0; + Data->qvel[SpinDof + 5] = 3.0; + DepthHoldTargetZ = -5.0; // 定深跟着走,别把 ROV 拉回出生高度 + // 关掉浮心高度:BG≠0 会形成"单摆"回复力矩,角速度会来回摆, + // 没法拟合成指数衰减。自检只要纯阻尼,浮力大小不变(仍然中性附近)。 + BuoyancyArm = 0.0f; + bDepthHoldActive = false; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=-5m,施加 3 rad/s 三轴角速度脉冲" + "(dof_damping=%.2f)"), + (double)Model->dof_damping[SpinDof + 3]); + } + if (bSpinApplied) { + const int32 Tick = (int32)(Now * 4.0); + if (Tick != SpinLogTick) { + SpinLogTick = Tick; + const double Wx = Data->qvel[SpinDof + 3]; + const double Wy = Data->qvel[SpinDof + 4]; + const double Wz = Data->qvel[SpinDof + 5]; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs wx=%.4f wy=%.4f wz=%.4f |w|=%.4f rad/s"), Now - 2.0, Wx, Wy, Wz, FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); + } + } + } + } + + // 命令行自检:-RovTiltTest 在 2 秒时把 ROV 倾斜 20° 后释放, + // 验证被动稳定(浮心高于重心产生的回复力矩)能否把它扶正。 + static const bool bTiltTest = FParse::Param(FCommandLine::Get(), TEXT("RovTiltTest")); + static bool bTiltApplied = false; + static int32 TiltLogTick = -1; + if (bTiltTest && Model->body_jntnum[BodyId] >= 1) { + const int32 TJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[TJnt] == mjJNT_FREE) { + const int32 TDof = Model->jnt_dofadr[TJnt]; + const int32 TQPos = Model->jnt_qposadr[TJnt]; + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + + if (!bTiltApplied && Now > 2.0) { + bTiltApplied = true; + const double HalfAngle = 10.0 * PI / 180.0; // 20° 的一半 + Data->qpos[TQPos + 2] = 4.0; // 抬到水中,避免接触干扰 + Data->qpos[TQPos + 3] = FMath::Cos(HalfAngle); // w + Data->qpos[TQPos + 4] = FMath::Sin(HalfAngle); // x(绕 X 轴倾斜) + Data->qpos[TQPos + 5] = 0.0; + Data->qpos[TQPos + 6] = 0.0; + for (int32 k = 0; k < 6; ++k) { + Data->qvel[TDof + k] = 0.0; + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][倾斜自检] 已倾斜 20° 并释放(BG=%.3f m)"), BuoyancyArm); + } + + if (bTiltApplied) { + const int32 Tick = (int32)(Now * 4.0); + if (Tick != TiltLogTick) { + TiltLogTick = Tick; + // 当前倾角:机体系 +Z 转到世界后与竖直方向的夹角 + const double Qw2 = Data->qpos[TQPos + 3]; + const double Qx2 = Data->qpos[TQPos + 4]; + const double Qy2 = Data->qpos[TQPos + 5]; + const double Qz2 = Data->qpos[TQPos + 6]; + // 旋转 (0,0,1):结果 z 分量 = 1 - 2*(x² + y²) + const double UpZ = 1.0 - 2.0 * (Qx2 * Qx2 + Qy2 * Qy2); + const double TiltDeg = FMath::RadiansToDegrees( + FMath::Acos(FMath::Clamp(UpZ, -1.0, 1.0))); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][倾斜自检] t=%.2fs 倾角=%.2f° ω=%.4f rad/s"), Now - 2.0, TiltDeg, Data->qvel[TDof + 4]); + } + } + } + } + + // 命令行自检:-RovTorqueAxis=0/1/2 只给该轴一个 100 N·m 力矩, + // 用来确定 qfrc_applied 角向分量到底在机体系还是世界系。 + static const bool bTorqueAxisTest = FParse::Param(FCommandLine::Get(), TEXT("RovTorqueAxisTest")); + static int32 TorqueTestAxis = -1; + if (bTorqueAxisTest && TorqueTestAxis < 0) { + int32 V = 0; + if (FParse::Value(FCommandLine::Get(), TEXT("RovTorqueAxis="), V)) { + TorqueTestAxis = V; + } + else { + TorqueTestAxis = 0; + } + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][力矩轴自检] 将只给轴 %d 施加 100 N·m"), TorqueTestAxis); + } + + if (bDepthHoldActive) { + DepthPID.MaxThrust = (float)ThrusterMax; + const float PidThrust = DepthPID.Compute( + (float)DepthHoldTargetZ, (float)RovZ, (float)FrameDt, (float)VCurrent.Z); + ThrusterZ = FMath::Clamp((double)PidThrust, -ThrusterMax, ThrusterMax); + + // 定深自检日志(放在 PID 之后,才能看到真实的 PID 输出) + if (bDepthHoldTest) { + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + const int32 Tick = (int32)(Now * 2.0); + if (Tick != DepthTestLastTick) { + DepthTestLastTick = Tick; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深自检] t=%.1fs 目标=%.3f 当前=%.3f 误差=%+.3f m " + "PID推力=%.1f N 洋流Vz=%.3f m/s"), + Now, + DepthHoldTargetZ, + RovZ, + DepthHoldTargetZ - RovZ, + ThrusterZ, + VCurrent.Z); + } + } + } + + // 【4】水面钳制:ROV 已在水面以上时,不允许推进器继续向上推 + if (RovZ >= WaterSurfaceZ && ThrusterZ > 0.0) { + ThrusterZ = 0.0; + } + + // 【5】浮力:Fb = ρ·g·V,永远向上。 + // 但**水面之上没有水**,所以浮力为零 —— 这也是 ROV 不会飞出水的物理原因: + // 一旦越过水面,只剩重力把它拉回来。 + double Buoyancy = 0.0; + if (RovZ < WaterSurfaceZ) { + Buoyancy = FluidDensity * Gravity * DisplacedVolume * BuoyancyScale; + } + + // 【6】洋流拖曳:Fc = -k·(V_robot - V_current) + // 用相对流速,而不是只用洋流速度 —— 机器人自己游动时也应该受到阻力。 + double CurrentX = 0.0, CurrentY = 0.0, CurrentZ = 0.0; + { + double Vrx = 0.0, Vry = 0.0, Vrz = 0.0; + if (Model->body_jntnum[BodyId] >= 1) { + const int32 JntId = Model->body_jntadr[BodyId]; + if (Model->jnt_type[JntId] == mjJNT_FREE) { + const int32 DofAdr = Model->jnt_dofadr[JntId]; + Vrx = Data->qvel[DofAdr + 0]; + Vry = Data->qvel[DofAdr + 1]; + Vrz = Data->qvel[DofAdr + 2]; + } + } + CurrentX = -CurrentDragCoeff * (Vrx - VCurrent.X); + CurrentY = -CurrentDragCoeff * (Vry - VCurrent.Y); + CurrentZ = -CurrentDragCoeff * (Vrz - VCurrent.Z); + + // 洋流诊断:每秒打印一次流速与拖曳力,确认量级 + if (bEnableOceanCurrent) { + static int32 OceanLogTick = -1; + const int32 Tick = (int32)(GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0); + if (Tick != OceanLogTick) { + OceanLogTick = Tick; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][洋流诊断] V_current=(%.4f,%.4f,%.4f) |Vc|=%.4f " + "V_robot=(%.4f,%.4f,%.4f) |Vr|=%.4f F=(%.1f,%.1f,%.1f) N"), + VCurrent.X, + VCurrent.Y, + VCurrent.Z, + VCurrent.Size(), + Vrx, + Vry, + Vrz, + FMath::Sqrt(Vrx * Vrx + Vry * Vry + Vrz * Vrz), + CurrentX, + CurrentY, + CurrentZ); + } + } + } + + // 【7】水动力阻力/升力:由 MuJoCo 椭球流体模型自行计算,这里不重复施加 + + const double Fx = ThrusterX + CurrentX; + const double Fy = ThrusterY + CurrentY; + const double Fz = ThrusterZ + CurrentZ; // 浮力单独走 mj_applyFT(作用在浮心) + + // 【8】浮力的回复力矩由 mj_applyFT 在浮心处施加时自动产生,这里不再手算。 + + // 【9】附加角阻尼:τ = -Kd·ω(机体系),与流体模型叠加。 + // Kd = AngularDampingCoeff × 平均主惯量。 + double TorqueX = 0.0, TorqueY = 0.0, TorqueZ = 0.0; + double WorldTorqueX = 0.0, WorldTorqueY = 0.0, WorldTorqueZ = 0.0; + if (AngularDampingCoeff > 0.0f && Model->body_jntnum[BodyId] >= 1) { + const int32 DampJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[DampJnt] == mjJNT_FREE) { + const int32 DampDof = Model->jnt_dofadr[DampJnt]; + // 用复合惯量:body_inertia 只含本体,不含机械臂。 + const double Iavg = (EffInertia[0] + EffInertia[1] + EffInertia[2]) / 3.0; + const double Kd = (double)AngularDampingCoeff * Iavg; + TorqueX += -Kd * Data->qvel[DampDof + 3]; + TorqueY += -Kd * Data->qvel[DampDof + 4]; + TorqueZ += -Kd * Data->qvel[DampDof + 5]; + } + } + + // 旧的四元数 PD 姿态控制器(已停用,保留备查)。 + // 误差 e = 2·sign(qw)·qv,增益按惯量缩放;由下方【10】级联控制器取代。 + if (false && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { + const int32 AttJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[AttJnt] == mjJNT_FREE) { + const int32 AttDof = Model->jnt_dofadr[AttJnt]; + + // 首次运行捕获当前航向作为目标 + if (!bAttitudeInitialized) { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), + 1.0 - 2.0 * (Qy * Qy + Qz * Qz)); + TargetYawDeg = FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][姿态] 已锁定目标航向 %.1f°,带宽 %.2f rad/s,阻尼比 %.2f"), TargetYawDeg, AttitudeBandwidth, AttitudeZeta); + } + + const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw); + const double Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t (机体系误差) + auto QMul = [](double aw, double ax, double ay, double az, double bw, double bx, double by, double bz, double& ow, double& ox, double& oy, double& oz) { + ow = aw * bw - ax * bx - ay * by - az * bz; + ox = aw * bx + ax * bw + ay * bz - az * by; + oy = aw * by - ax * bz + ay * bw + az * bx; + oz = aw * bz + ax * by - ay * bx + az * bw; + }; + double Qew = 0.0, Qex = 0.0, Qey = 0.0, Qez = 0.0; + QMul(Qw, -Qx, -Qy, -Qz, Qtw, 0.0, 0.0, Qtz, Qew, Qex, Qey, Qez); + if (Qew < 0.0) { // 取短弧 + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; + } + + const double E[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + + // 积分(限幅防饱和) + double IntArr[3] = { (double)AttitudeIntegral.X, + (double)AttitudeIntegral.Y, + (double)AttitudeIntegral.Z }; + for (int32 Axis = 0; Axis < 3; ++Axis) { + IntArr[Axis] += E[Axis] * FrameDt; + IntArr[Axis] = FMath::Clamp(IntArr[Axis], -2.0, 2.0); + } + AttitudeIntegral = FVector(IntArr[0], IntArr[1], IntArr[2]); + + const double W = (double)AttitudeBandwidth; + const double Zeta = (double)AttitudeZeta; + const double KiF = (double)AttitudeIntegralFactor; + + // body_inertia 是**主轴系**下的主值,body_iquat 给出主轴相对机体的旋转。 + // 直接用 body_inertia[3*BodyId+Axis] 当 X/Y/Z 惯量是错的(除非 iquat 是单位四元数)。 + // 正确做法:把机体单位轴转到主轴系,算 vᵀ·diag(I)·v 得到该轴的等效惯量。 + const mjtNum* IQ = &Model->body_iquat[4 * BodyId]; + double Iaxis[3] = { 0.0, 0.0, 0.0 }; + for (int32 a = 0; a < 3; ++a) { + double vx = (a == 0) ? 1.0 : 0.0; + double vy = (a == 1) ? 1.0 : 0.0; + double vz = (a == 2) ? 1.0 : 0.0; + double px = 0.0, py = 0.0, pz = 0.0; + RotByQuat(IQ[0], -IQ[1], -IQ[2], -IQ[3], vx, vy, vz, px, py, pz); + Iaxis[a] = Model->body_inertia[3 * BodyId + 0] * px * px + Model->body_inertia[3 * BodyId + 1] * py * py + Model->body_inertia[3 * BodyId + 2] * pz * pz; + } + + static bool bLoggedIquat = false; + if (!bLoggedIquat) { + bLoggedIquat = true; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][惯量诊断] body_iquat=(%.4f, %.4f, %.4f, %.4f) " + "主轴主值=(%.3f, %.3f, %.3f) 机体轴等效惯量=(%.3f, %.3f, %.3f)"), + IQ[0], + IQ[1], + IQ[2], + IQ[3], + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + Iaxis[0], + Iaxis[1], + Iaxis[2]); + } + + // 姿态诊断:每秒打印一次欧拉角、目标、误差、角速度和力矩 + { + static int32 AttLogTick = -1; + const int32 TickNow = (int32)(GetWorld()->GetTimeSeconds()); + if (TickNow != AttLogTick) { + AttLogTick = TickNow; + const double RollD = FMath::RadiansToDegrees(FMath::Atan2(2.0 * (Qw * Qx + Qy * Qz), 1.0 - 2.0 * (Qx * Qx + Qy * Qy))); + const double PitchD = FMath::RadiansToDegrees(FMath::Asin(FMath::Clamp(2.0 * (Qw * Qy - Qz * Qx), -1.0, 1.0))); + const double YawD = FMath::RadiansToDegrees(FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), 1.0 - 2.0 * (Qy * Qy + Qz * Qz))); + UE_LOG(LogRovBridge, Log, TEXT("[%s][姿态诊断] roll=%.1f pitch=%.1f yaw=%.1f targetYaw=%.1f |E|=(%.3f,%.3f,%.3f) omega=(%.2f,%.2f,%.2f)"), TEXT("RovMuJoCoBridge"), RollD, PitchD, YawD, TargetYawDeg, E[0], E[1], E[2], Data->qvel[AttDof + 3], Data->qvel[AttDof + 4], Data->qvel[AttDof + 5]); + } + } + + for (int32 Axis = 0; Axis < 3; ++Axis) { + // 优先用复合惯量(含机械臂);取不到时退回本体的等效惯量 + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] : Iaxis[Axis]; + const double Kp = I * W * W; + const double Kd = 2.0 * I * Zeta * W; + const double Ki = I * W * W * W * KiF; + const double Omega = Data->qvel[AttDof + 3 + Axis]; + double T = Kp * E[Axis] + Ki * IntArr[Axis] - Kd * Omega; + T = FMath::Clamp(T, -1500.0, 1500.0); // 力矩限幅,防止饱和发散 + if (Axis == 0) { + TorqueX += T; + } + else if (Axis == 1) { + TorqueY += T; + } + else { + TorqueZ += T; + } + } + } + } + + // 【10】级联姿态控制(已迁移到物理线程,见 ComputeAttitudeTorque) + // 保留本块仅作参考:它按渲染帧调用,帧率低于约 25 fps 时会与积分项形成 + // 慢振荡直至翻转,因此改为在物理线程按 500 Hz 运行。 + // 外环:姿态误差 -> 期望角速度(机体系) + // 内环:期望角速度 - 实测角速度(qvel[3:6] 为机体系)-> 力矩 + if (false && bEnableAttitudeHold && !bSpinTest && Model->body_jntnum[BodyId] >= 1) { + const int32 NJ = Model->body_jntadr[BodyId]; + if (Model->jnt_type[NJ] == mjJNT_FREE) { + const int32 ND = Model->jnt_dofadr[NJ]; + const double* QQ = &Data->xquat[4 * BodyId]; + const double qw = QQ[0], qx = QQ[1], qy = QQ[2], qz = QQ[3]; + + if (!bAttitudeInitialized) { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz)); + TargetYawDeg = FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][姿态] 级联控制启用,锁定航向 %.1f°"), TargetYawDeg); + } + + const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw); + const double Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t (机体系),取短弧 + const double aw = qw, ax = -qx, ay = -qy, az = -qz; + double Qew = aw * Qtw - ax * 0.0 - ay * 0.0 - az * Qtz; + double Qex = aw * 0.0 + ax * Qtw + ay * Qtz - az * 0.0; + double Qey = aw * 0.0 - ax * Qtz + ay * Qtw + az * 0.0; + double Qez = aw * Qtz + ax * 0.0 - ay * 0.0 + az * Qtw; + if (Qew < 0.0) { + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; + } + + const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + const double KpAtt[3] = { 1.5, 1.5, 1.0 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wn = 5.0; + static double AttInt[3] = { 0.0, 0.0, 0.0 }; + + // 积分步长必须用「距上次调用实际经过的仿真时间」,不能用固定 timestep: + // ApplyControlForce 是按渲染帧调用的,帧率不同积分速度就不同 + // (Development 约 60 fps、Shipping 约 120+ fps)。用固定 timestep 会让 + // 打包版的积分快一倍,恒力矩还没抵掉就先过冲,表现为缓慢翻转。 + static double LastIntTime = -1.0; + double IntDt = 0.0; + { + const double NowT = (double)Data->time; + if (LastIntTime >= 0.0) { + IntDt = FMath::Clamp(NowT - LastIntTime, 0.0, 0.05); + } + LastIntTime = NowT; + } + + // 航向前馈(见【航向调整】):按住 ←/→ 时直接给期望角速度。 + // 上限也要一起放大,否则前馈会被 Wmax 截掉,目标航向又会跑飞。 + const double YawFF = FMath::DegreesToRadians(YawCmdRateDeg); + const double WmaxZ = (YawFF != 0.0) + ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) + : Wmax[2]; + + for (int32 Axis = 0; Axis < 3; ++Axis) { + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] + : Model->body_inertia[3 * BodyId + Axis]; + const double Wlim = (Axis == 2) ? WmaxZ : Wmax[Axis]; + // 积分项:抵消模型里无法消除的恒定力矩(质心/浮心不重合、 + // 机械臂残余重量等),否则纯 P 会停在一个固定倾角上。 + AttInt[Axis] = FMath::Clamp(AttInt[Axis] + E3[Axis] * IntDt, -2.0, 2.0); + double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + AttitudeIntegralGain * AttInt[Axis], + -Wlim, + Wlim); + if (Axis == 2 && YawFF != 0.0) { + Wsp = FMath::Clamp(Wsp + YawFF, -WmaxZ, WmaxZ); + } + double T = I * Wn * (Wsp - Data->qvel[ND + 3 + Axis]); + T = FMath::Clamp(T, -1500.0, 1500.0); + if (Axis == 0) { + TorqueX += T; + } + else if (Axis == 1) { + TorqueY += T; + } + else { + TorqueZ += T; + } + } + + // 航向诊断(每秒一次):目标 / 实测 / 误差 / 机体 Z 角速度 / 前馈 + { + static double LastYawLogT = -1.0; + if (Data->time - LastYawLogT > 1.0) { + LastYawLogT = Data->time; + const double CurYawDeg = FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz))); + double YawErrDeg = (double)TargetYawDeg - CurYawDeg; + while (YawErrDeg > 180.0) { + YawErrDeg -= 360.0; + } + while (YawErrDeg < -180.0) { + YawErrDeg += 360.0; + } + // 真实倾角:机体 Z 轴与世界 Z 的夹角,不受欧拉角约定影响 + const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); + const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); + UE_LOG(LogRovBridge, Log, TEXT("[%s][姿态诊断] t=%.1f 倾角=%.1f° 目标=%.1f° 实测=%.1f° 误差=%.1f° " + "E3=(%.3f, %.3f, %.3f) w=(%.3f, %.3f, %.3f) 前馈=%+.0f°/s"), + TEXT("RovMuJoCoBridge"), + Data->time, + TiltDeg, + TargetYawDeg, + CurYawDeg, + YawErrDeg, + E3[0], + E3[1], + E3[2], + Data->qvel[ND + 3], + Data->qvel[ND + 4], + Data->qvel[ND + 5], + YawCmdRateDeg); + } + } + } + } + // 【11】机械臂静力矩配平:常值前馈,把控制器硬扛的那部分稳态力矩抵掉 + if (bEnableAttitudeHold && !bSpinTest) { + float TrimPitch = TrimTorquePitch; + float TrimRoll = TrimTorqueRoll; + FParse::Value(FCommandLine::Get(), TEXT("RovTrimPitch="), TrimPitch); + FParse::Value(FCommandLine::Get(), TEXT("RovTrimRoll="), TrimRoll); + TorqueX += TrimRoll; + TorqueY += TrimPitch; + } + + CachedBodyId.store(BodyId); + CachedControlTime.store((double)Data->time); + CachedForceX.store(Fx); + CachedForceY.store(Fy); + CachedForceZ.store(Fz); + + // 力矩轴自检:覆盖掉所有力矩项,只给指定轴 100 N·m + if (bTorqueAxisTest && TorqueTestAxis >= 0) { + TorqueX = (TorqueTestAxis == 0) ? 100.0 : 0.0; + TorqueY = (TorqueTestAxis == 1) ? 100.0 : 0.0; + TorqueZ = (TorqueTestAxis == 2) ? 100.0 : 0.0; + + static int32 TorqueLogTick = -1; + const int32 Tick = (int32)(GetWorld()->GetTimeSeconds() * 4.0); + if (Tick != TorqueLogTick && Model->body_jntnum[BodyId] >= 1) { + TorqueLogTick = Tick; + const int32 J = Model->body_jntadr[BodyId]; + if (Model->jnt_type[J] == mjJNT_FREE) { + const int32 D = Model->jnt_dofadr[J]; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][力矩轴自检] 给轴%d 100N·m -> omega=(%.4f, %.4f, %.4f)"), TorqueTestAxis, Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5]); + } + } + } + + // 力矩就是机体系,直接缓存;ApplyCachedForce 里直写 qfrc_applied[3..5] + CachedTorqueX.store(TorqueX); + CachedTorqueY.store(TorqueY); + CachedTorqueZ.store(TorqueZ); + CachedBuoyancy.store(Buoyancy); + + if (!bControlCallbackRegistered) { + TWeakObjectPtr WeakThis(this); + Mgr->PhysicsEngine->RegisterPreStepCallback( + [WeakThis](mjModel* M, mjData* D) { + if (URovMuJoCoBridge* Self = WeakThis.Get()) { + Self->ApplyCachedForce(M, D); + } + }); + bControlCallbackRegistered = true; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已注册物理线程施力回调(PreStepCallbacks)")); + } + + // 节流日志 + static int32 CtrlLogCounter = 0; + if ((LocalX != 0.0 || LocalY != 0.0 || LocalZ != 0.0) && (++CtrlLogCounter % 30 == 0)) { + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][受力] 输入=(%.0f,%.0f,%.0f) 推力=(%.1f,%.1f,%.1f) N " + "浮力=%.1f N 洋流拖曳=(%.1f,%.1f,%.1f) N 合计Fz=%.1f N " + "洋流=(%.2f,%.2f,%.2f) m/s 定深=%d Z=%.3f"), + LocalX, + LocalY, + LocalZ, + ThrusterX, + ThrusterY, + ThrusterZ, + Buoyancy, + CurrentX, + CurrentY, + CurrentZ, + Fz, + VCurrent.X, + VCurrent.Y, + VCurrent.Z, + bDepthHoldActive ? 1 : 0, + RovZ); + } +} + +void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int32 BodyId, double Dt, double OutTorque[3]) +{ + OutTorque[0] = 0.0; + OutTorque[1] = 0.0; + OutTorque[2] = 0.0; + + if (!bEnableAttitudeHold || !Model || !Data || BodyId < 0 || BodyId >= Model->nbody) { + return; + } + if (Model->body_jntnum[BodyId] < 1) { + return; + } + const int32 J = Model->body_jntadr[BodyId]; + if (Model->jnt_type[J] != mjJNT_FREE) { + return; + } + const int32 D = Model->jnt_dofadr[J]; + + const double* QQ = &Data->xquat[4 * BodyId]; + const double qw = QQ[0], qx = QQ[1], qy = QQ[2], qz = QQ[3]; + + if (!bAttitudeInitialized) { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz)); + TargetYawDeg = (float)FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][姿态] 物理线程级联控制启用(%.0f Hz),锁定航向 %.1f°"), 1.0 / FMath::Max(Dt, 1e-6), TargetYawDeg); + } + + const double HalfYaw = FMath::DegreesToRadians((double)TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw), Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t(机体系),取短弧 + const double aw = qw, ax = -qx, ay = -qy, az = -qz; + double Qew = aw * Qtw - az * Qtz; + double Qex = ax * Qtw + ay * Qtz; + double Qey = -ax * Qtz + ay * Qtw; + double Qez = aw * Qtz + az * Qtw; + if (Qew < 0.0) { + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; + } + + const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + const double KpAtt[3] = { 1.5, 1.5, 1.0 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wn = 5.0; + + const double YawFF = CachedYawFFRadPerSec.load(); + const double WmaxZ = (YawFF != 0.0) ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) : Wmax[2]; + + for (int32 Axis = 0; Axis < 3; ++Axis) { + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] + : Model->body_inertia[3 * BodyId + Axis]; + const double Wlim = (Axis == 2) ? WmaxZ : Wmax[Axis]; + + // 大角度时临时抬高角速度上限,给回正留出力矩余量。 + // 稳态扰动力矩约 86 N·m;若上限恒为 0.8 rad/s,最大回正力矩只有约 86 N·m, + // 一旦翻过 90° 就会停在「控制器力矩 = 重力矩」的平衡点上,再也回不来。 + const double Boost = FMath::Clamp((FMath::Abs(E3[Axis]) - 0.30) * 1.5, 0.0, 2.0); + const double WlimEff = Wlim + Boost; + + // 积分限幅 + 抗饱和:角速度大时不再累积,并按时间常数快速泄放。 + // 只按角速度判据:若再叠加「误差小才积分」,稳态大倾角就永远消不掉 + // (实测会停在 28° 下不来)。 + const double Omega = (double)Data->qvel[D + 3 + Axis]; + if (FMath::Abs(Omega) < 0.30) { + AttIntState[Axis] = FMath::Clamp(AttIntState[Axis] + E3[Axis] * Dt, -2.0, 2.0); + } + else { + AttIntState[Axis] *= FMath::Max(0.0, 1.0 - 3.0 * Dt); + } + + double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + (double)AttitudeIntegralGain * AttIntState[Axis], + -WlimEff, + WlimEff); + if (Axis == 2 && YawFF != 0.0) { + Wsp = FMath::Clamp(Wsp + YawFF, -WlimEff, WlimEff); + } + double T = I * Wn * (Wsp - Omega); + OutTorque[Axis] = FMath::Clamp(T, -1500.0, 1500.0); + } + + // 1 Hz 诊断:倾角、误差、角速度、力矩、积分量 + { + static double LastLogT = -1.0; + if (Data->time - LastLogT > 1.0) { + LastLogT = Data->time; + const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); + const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][姿态环] t=%.1f 倾角=%.2f E3=(%+.3f,%+.3f,%+.3f) " + "w=(%+.3f,%+.3f,%+.3f) T=(%+.1f,%+.1f,%+.1f) 积分=(%+.2f,%+.2f,%+.2f)"), + Data->time, + TiltDeg, + E3[0], + E3[1], + E3[2], + Data->qvel[D + 3], + Data->qvel[D + 4], + Data->qvel[D + 5], + OutTorque[0], + OutTorque[1], + OutTorque[2], + AttIntState[0], + AttIntState[1], + AttIntState[2]); + } + } +} + +void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) +{ + const int32 BodyId = CachedBodyId.load(); + if (BodyId < 0 || !Model || !Data || BodyId >= Model->nbody) { + return; + } + + // 力与力矩统一用 mj_applyFT 施加:force / torque / point 分开传参、全部世界系, + // 自动补 r×F,避免 xfrc_applied 的元素顺序歧义,也能让浮力作用在浮心。 + if (Model->body_jntnum[BodyId] < 1) { + return; + } + const int32 JntId = Model->body_jntadr[BodyId]; + if (Model->jnt_type[JntId] != mjJNT_FREE) { + return; + } + const int32 DofAdr = Model->jnt_dofadr[JntId]; + + // 逐接触点日志(4.2.4):此处读到的是上一步求解出的接触 + LogContacts(Model, Data); + + // mj_applyFT 是累加语义,先清零本 body 的 6 维广义力 + for (int32 k = 0; k < 6; ++k) { + Data->qfrc_applied[DofAdr + k] = 0.0; + } + + // 指令新鲜度:控制量在游戏线程按帧更新,物理线程按 500 Hz 步进。 + // 游戏线程卡顿时旧指令会被持续施加,这里按仿真时间做淡出: + // 超过 0.04 s 开始衰减,超过 0.08 s 完全交给物理线程的兜底控制器。 + // 阈值取得比 0.1 s 更紧,是因为帧率低于约 25 fps 时游戏线程的姿态环 + // 已经跟不上(每帧只更新一次),会与积分项形成慢振荡直至翻转。 + const double CmdAge = (double)Data->time - CachedControlTime.load(); + const double Fresh = FMath::Clamp(1.0 - (CmdAge - 0.04) / 0.04, 0.0, 1.0); + + const mjtNum CG[3] = { + Data->xipos[3 * BodyId + 0], + Data->xipos[3 * BodyId + 1], + Data->xipos[3 * BodyId + 2] + }; + const mjtNum ZeroT[3] = { 0.0, 0.0, 0.0 }; + const mjtNum ZeroF[3] = { 0.0, 0.0, 0.0 }; + + // 【1】推进器推力 + 洋流拖曳:作用在质心 + mjtNum F1[3] = { + (mjtNum)(CachedForceX.load() * Fresh), + (mjtNum)(CachedForceY.load() * Fresh), + (mjtNum)(CachedForceZ.load() * Fresh - ContactTestForce) + }; + mj_applyFT(Model, Data, F1, ZeroT, CG, BodyId, Data->qfrc_applied); + + // 【1b】显式二次阻力:沿机体系三轴 F = -0.5·ρ·Cd·A·|v|·v。 + // 用于补偿椭球近似对大钝体 / 细长体的偏差,默认关闭。 + if (bUseExplicitDrag) { + const double QW = Data->xquat[4 * BodyId + 0]; + const double QX = Data->xquat[4 * BodyId + 1]; + const double QY = Data->xquat[4 * BodyId + 2]; + const double QZ = Data->xquat[4 * BodyId + 3]; + double Bx = 0.0, By = 0.0, Bz = 0.0; + RotByQuat(QW, -QX, -QY, -QZ, Data->qvel[DofAdr + 0], Data->qvel[DofAdr + 1], Data->qvel[DofAdr + 2], Bx, By, Bz); + const double Half = 0.5 * (double)FluidDensity; + const double Fbx = -Half * (double)DragCoeff.X * (double)DragRefArea.X * FMath::Abs(Bx) * Bx; + const double Fby = -Half * (double)DragCoeff.Y * (double)DragRefArea.Y * FMath::Abs(By) * By; + const double Fbz = -Half * (double)DragCoeff.Z * (double)DragRefArea.Z * FMath::Abs(Bz) * Bz; + double Fwx = 0.0, Fwy = 0.0, Fwz = 0.0; + RotByQuat(QW, QX, QY, QZ, Fbx, Fby, Fbz, Fwx, Fwy, Fwz); + mjtNum F1b[3] = { (mjtNum)Fwx, (mjtNum)Fwy, (mjtNum)Fwz }; + mj_applyFT(Model, Data, F1b, ZeroT, CG, BodyId, Data->qfrc_applied); + } + + // 【1c】附加质量:F = -m_A·a(机体系三轴),默认关闭。 + // 椭球模型内部已含附加质量,本项用于单独调节而不改模型。 + if (bUseAddedMass) { + const double QW = Data->xquat[4 * BodyId + 0]; + const double QX = Data->xquat[4 * BodyId + 1]; + const double QY = Data->xquat[4 * BodyId + 2]; + const double QZ = Data->xquat[4 * BodyId + 3]; + double Ax = 0.0, Ay = 0.0, Az = 0.0; + RotByQuat(QW, -QX, -QY, -QZ, Data->qacc[DofAdr + 0], Data->qacc[DofAdr + 1], Data->qacc[DofAdr + 2], Ax, Ay, Az); + const double Fbx = -(double)AddedMass.X * Ax; + const double Fby = -(double)AddedMass.Y * Ay; + const double Fbz = -(double)AddedMass.Z * Az; + double Fwx = 0.0, Fwy = 0.0, Fwz = 0.0; + RotByQuat(QW, QX, QY, QZ, Fbx, Fby, Fbz, Fwx, Fwy, Fwz); + mjtNum F1c[3] = { (mjtNum)Fwx, (mjtNum)Fwy, (mjtNum)Fwz }; + mj_applyFT(Model, Data, F1c, ZeroT, CG, BodyId, Data->qfrc_applied); + } + + // 【2】浮力:作用在整机浮心 = 整机合成质心 + R·(0,0,BG)。 + // 排水体积属于整个载体,浮心应跟随整机质量分布,而非艇体自身质心。 + const double Buoy = CachedBuoyancy.load(); + + double TotalMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; + for (int32 b = 0; b < Model->nbody; ++b) { + bool bInSubtree = (b == BodyId); + if (!bInSubtree) { + int32 p = Model->body_parentid[b]; + while (p > 0) { + if (p == BodyId) { + bInSubtree = true; + break; + } + p = Model->body_parentid[p]; + } + } + if (!bInSubtree) { + continue; + } + const double m = Model->body_mass[b]; + TotalMass += m; + Cx += m * Data->xipos[3 * b + 0]; + Cy += m * Data->xipos[3 * b + 1]; + Cz += m * Data->xipos[3 * b + 2]; + } + if (TotalMass > 1e-6) { + Cx /= TotalMass; + Cy /= TotalMass; + Cz /= TotalMass; + } + else { + Cx = CG[0]; + Cy = CG[1]; + Cz = CG[2]; + } + // 优先使用 MuJoCo 计算的子树质心,保证与重力作用点一致。 + if (Data->subtree_com) { + Cx = Data->subtree_com[3 * BodyId + 0]; + Cy = Data->subtree_com[3 * BodyId + 1]; + Cz = Data->subtree_com[3 * BodyId + 2]; + } + + if (Buoy > 0.0) { + double Rx = 0.0, Ry = 0.0, Rz = 0.0; + RotByQuat(Data->xquat[4 * BodyId + 0], Data->xquat[4 * BodyId + 1], Data->xquat[4 * BodyId + 2], Data->xquat[4 * BodyId + 3], 0.0, 0.0, (double)BuoyancyArm, Rx, Ry, Rz); + mjtNum F2[3] = { 0.0, 0.0, (mjtNum)Buoy }; + mjtNum CBw[3] = { + (mjtNum)(Cx + Rx), + (mjtNum)(Cy + Ry), + (mjtNum)(Cz + Rz) + }; + mj_applyFT(Model, Data, F2, ZeroT, CBw, BodyId, Data->qfrc_applied); + } + + // 【2b】机械臂浮力:按排水体积在臂的合成质心处施加向上浮力, + // 等价于给机械臂绑浮力块。 + if (ArmBuoyancyVolume > 0.0f) { + double ArmMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; + for (int32 b = 0; b < Model->nbody; ++b) { + int32 p = Model->body_parentid[b]; + bool bDescendant = false; + while (p > 0) { + if (p == BodyId) { + bDescendant = true; + break; + } + p = Model->body_parentid[p]; + } + if (!bDescendant) { + continue; + } + const double m = Model->body_mass[b]; + ArmMass += m; + Cx += m * Data->xipos[3 * b + 0]; + Cy += m * Data->xipos[3 * b + 1]; + Cz += m * Data->xipos[3 * b + 2]; + } + if (ArmMass > 1e-6) { + const double VolM3 = (double)ArmBuoyancyVolume * 0.001 * (double)ArmBuoyancyScale; + const double FArm = (double)FluidDensity * 9.81 * VolM3; + mjtNum F3[3] = { 0.0, 0.0, (mjtNum)FArm }; + mjtNum CoB[3] = { + (mjtNum)(Cx / ArmMass), + (mjtNum)(Cy / ArmMass), + (mjtNum)(Cz / ArmMass) + }; + mj_applyFT(Model, Data, F3, ZeroT, CoB, BodyId, Data->qfrc_applied); + } + } + + // 【3】姿态力矩:free joint 的 qfrc_applied[3..5] 就是机体系力矩,直接赋值。 + // 姿态环已移到物理线程(ComputeAttitudeTorque,500 Hz),这里只叠加 + // 游戏线程给的配平力矩与角阻尼,按 Fresh 淡出。 + double Tx = CachedTorqueX.load() * Fresh; + double Ty = CachedTorqueY.load() * Fresh; + double Tz = CachedTorqueZ.load() * Fresh; + + double AttT[3] = { 0.0, 0.0, 0.0 }; + ComputeAttitudeTorque(Model, Data, BodyId, (double)Model->opt.timestep, AttT); + + // 自检:-RovFlipTest 在 6~6.8 s 施加 500 N·m 横滚力矩把机身掀翻,用于验证回正能力 + static const bool bFlipTest = FParse::Param(FCommandLine::Get(), TEXT("RovFlipTest")); + if (bFlipTest && Data->time > 6.0 && Data->time < 6.8) { + AttT[0] += 500.0; + } + Tx += AttT[0]; + Ty += AttT[1]; + Tz += AttT[2]; + + Data->qfrc_applied[DofAdr + 3] = (mjtNum)Tx; + Data->qfrc_applied[DofAdr + 4] = (mjtNum)Ty; + Data->qfrc_applied[DofAdr + 5] = (mjtNum)Tz; + + // 【4】一次性初始化:implicit 积分器 + 转动 dof 阻尼 + static bool bCtlInitDone = false; + if (!bCtlInitDone) { + bCtlInitDone = true; + Model->opt.integrator = mjINT_IMPLICIT; + const double Iavg = 0.5 * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0; + // 阻尼取值优先级:命令行清零(做流体基线对照)> 属性 > 0.5×平均惯量兜底 + static const bool bNoDofDamp = FParse::Param(FCommandLine::Get(), TEXT("RovNoDofDamp")); + double Idamp = (AngularDofDamping > 0.0f) ? (double)AngularDofDamping : Iavg; + if (bNoDofDamp) { + Idamp = 0.0; + } + Model->dof_damping[DofAdr + 3] = Idamp; + Model->dof_damping[DofAdr + 4] = Idamp; + Model->dof_damping[DofAdr + 5] = Idamp; + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][版本] mj_versionString=%s"), UTF8_TO_TCHAR(mj_versionString())); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][惯量] body_inertia=(%.3f, %.3f, %.3f) EffInertia=(%.3f, %.3f, %.3f) body_mass=%.3f"), Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], Model->body_inertia[3 * BodyId + 2], EffInertia[0], EffInertia[1], EffInertia[2], Model->body_mass[BodyId]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][控制] integrator=implicit dof_damping=%.2f N.m.s/rad"), Idamp); + + // 机械臂质量分布诊断:-RovArmDiag + // 目的:确认「臂子树 181 kg」是怎么来的(menagerie 原版 UR5e + 2F85 只有 ~21.9 kg), + // 顺便把每个连杆的质量/位置/碰撞体尺寸打出来,判断是不是整体被放大了。 + static const bool bArmDiag = FParse::Param(FCommandLine::Get(), TEXT("RovArmDiag")); + if (bArmDiag) { + const double* RovCg = &Data->xipos[3 * BodyId]; + double ArmMass = 0.0, Mx = 0.0, My = 0.0, Mz = 0.0; + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][臂诊断] ROV body[%d]='%s' mass=%.3f subtree=%.3f 惯量=(%.3f, %.3f, %.3f)"), BodyId, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[BodyId]), Model->body_mass[BodyId], Model->body_subtreemass[BodyId], Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], Model->body_inertia[3 * BodyId + 2]); + for (int32 b = 0; b < Model->nbody; ++b) { + int32 p = Model->body_parentid[b]; + bool bDescendant = false; + while (p > 0) { + if (p == BodyId) { + bDescendant = true; + break; + } + p = Model->body_parentid[p]; + } + if (!bDescendant) { + continue; + } + + const double m = Model->body_mass[b]; + const double* x = &Data->xipos[3 * b]; + const double rx = x[0] - RovCg[0], ry = x[1] - RovCg[1], rz = x[2] - RovCg[2]; + ArmMass += m; + Mx += m * rx; + My += m * ry; + Mz += m * rz; + UE_LOG(LogRovBridge, Log, TEXT(" [臂] body[%d]='%s' mass=%.4f 相对ROV重心=(%.4f, %.4f, %.4f) m 惯量=(%.5f, %.5f, %.5f)"), b, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[b]), m, rx, ry, rz, Model->body_inertia[3 * b + 0], Model->body_inertia[3 * b + 1], Model->body_inertia[3 * b + 2]); + + // 该 body 的碰撞几何尺寸,用来判断几何有没有被整体放大 + for (int32 g = 0; g < Model->ngeom; ++g) { + if (Model->geom_bodyid[g] != b) { + continue; + } + UE_LOG(LogRovBridge, Log, TEXT(" [geom] '%s' type=%d size=(%.4f, %.4f, %.4f) mass=%.4f"), UTF8_TO_TCHAR(Model->names + Model->name_geomadr[g]), (int32)Model->geom_type[g], Model->geom_size[3 * g + 0], Model->geom_size[3 * g + 1], Model->geom_size[3 * g + 2], Model->body_mass[b]); + } + } + if (ArmMass > 1e-6) { + const double G = 9.81; + // 重力矩 = Σ r × (0,0,-mg) = (-g·Σm·ry, +g·Σm·rx, 0) + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][臂诊断] 合计 mass=%.3f 合成质心相对ROV=(%.4f, %.4f, %.4f) m " + "重力矩=(%.1f, %.1f, %.1f) N·m 重量=%.1f N"), + ArmMass, + Mx / ArmMass, + My / ArmMass, + Mz / ArmMass, + -G * My, + G * Mx, + 0.0, + ArmMass * G); + } + } + } + + // 【5】qfrc_applied 累加监测(每秒一次) + { + static double LastQfrcLogT = -1.0; + if (Data->time - LastQfrcLogT > 1.0) { + LastQfrcLogT = Data->time; + const double* qq = &Data->xquat[4 * BodyId]; + const double RollDeg = FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[1] + qq[2] * qq[3]), + 1.0 - 2.0 * (qq[1] * qq[1] + qq[2] * qq[2]))); + const double PitchDeg = FMath::RadiansToDegrees( + FMath::Asin(FMath::Clamp(2.0 * (qq[0] * qq[2] - qq[3] * qq[1]), -1.0, 1.0))); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][qfrc] t=%.2f applied=(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) " + "roll=%.1f° pitch=%.1f° Z=%.3f ncon=%d bias=(%.1f, %.1f, %.1f) " + "constr=(%.1f, %.1f, %.1f)"), + Data->time, + Data->qfrc_applied[DofAdr + 0], + Data->qfrc_applied[DofAdr + 1], + Data->qfrc_applied[DofAdr + 2], + Data->qfrc_applied[DofAdr + 3], + Data->qfrc_applied[DofAdr + 4], + Data->qfrc_applied[DofAdr + 5], + RollDeg, + PitchDeg, + Data->xpos[3 * BodyId + 2], + (int32)Data->ncon, + Data->qfrc_bias[DofAdr + 3], + Data->qfrc_bias[DofAdr + 4], + Data->qfrc_bias[DofAdr + 5], + Data->qfrc_constraint[DofAdr + 3], + Data->qfrc_constraint[DofAdr + 4], + Data->qfrc_constraint[DofAdr + 5]); + } + } + // 【6】实时状态缓存:供 HUD 与外部读取(3.4 / 7.3.1) + { + const double* qq = &Data->xquat[4 * BodyId]; + CachedRollDeg.store(FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[1] + qq[2] * qq[3]), + 1.0 - 2.0 * (qq[1] * qq[1] + qq[2] * qq[2])))); + CachedPitchDeg.store(FMath::RadiansToDegrees( + FMath::Asin(FMath::Clamp(2.0 * (qq[0] * qq[2] - qq[3] * qq[1]), -1.0, 1.0)))); + CachedYawDeg.store(FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[3] + qq[1] * qq[2]), + 1.0 - 2.0 * (qq[2] * qq[2] + qq[3] * qq[3])))); + CachedDepthM.store((double)WaterSurfaceZ - Data->xpos[3 * BodyId + 2]); + CachedVelocityX.store(Data->qvel[DofAdr + 0]); + CachedVelocityY.store(Data->qvel[DofAdr + 1]); + CachedVelocityZ.store(Data->qvel[DofAdr + 2]); + const double Fx = CachedForceX.load() * Fresh; + const double Fy = CachedForceY.load() * Fresh; + const double Fz = CachedForceZ.load() * Fresh; + CachedThrustN.store(FMath::Sqrt(Fx * Fx + Fy * Fy + Fz * Fz)); + + // 接触力:只统计涉及本体的接触,并累加法向力方向 + double ContactSum = 0.0, Px = 0.0, Py = 0.0, Pz = 0.0; + double Dx = 0.0, Dy = 0.0, Dz = 0.0; + mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; + for (int32 i = 0; i < Data->ncon; ++i) { + const mjContact& C = Data->contact[i]; + const int32 B1 = Model->geom_bodyid[C.geom1]; + const int32 B2 = Model->geom_bodyid[C.geom2]; + bool bTouchesBody = false; + bool bBody1IsRov = false; + for (int32 b : { B1, B2 }) { + bool bInSubtree = false; + int32 p = b; + while (p > 0) { + if (p == BodyId) { + bInSubtree = true; + break; + } + p = Model->body_parentid[p]; + } + if (b == BodyId) { + bInSubtree = true; + } + if (bInSubtree) { + bTouchesBody = true; + if (b == B1) { + bBody1IsRov = true; + } + break; + } + } + if (!bTouchesBody) { + continue; + } + mj_contactForce(Model, Data, i, F6); + ContactSum += FMath::Abs((double)F6[0]); + // frame[0..2] 是从 geom1 指向 geom2 的法向;作用在本体上的方向取反 + const double Sign = bBody1IsRov ? -1.0 : 1.0; + Dx += Sign * C.frame[0] * F6[0]; + Dy += Sign * C.frame[1] * F6[0]; + Dz += Sign * C.frame[2] * F6[0]; + Px = C.pos[0]; + Py = C.pos[1]; + Pz = C.pos[2]; + } + CachedContactForce.store(ContactSum); + // 接触点存成「相对机体、UE 轴、厘米」的偏移:HUD 直接用 + // 机体 UE 位置 + 该偏移即可,避免 MuJoCo/UE 两套坐标混用。 + const double BX = Data->xpos[3 * BodyId + 0]; + const double BY = Data->xpos[3 * BodyId + 1]; + const double BZ = Data->xpos[3 * BodyId + 2]; + CachedContactPointX.store((Px - BX) * 100.0); + CachedContactPointY.store(-(Py - BY) * 100.0); + CachedContactPointZ.store((Pz - BZ) * 100.0); + const double DNorm = FMath::Sqrt(Dx * Dx + Dy * Dy + Dz * Dz); + if (DNorm > 1e-9) { + // 法向同样做 Y 镜像,得到 UE 世界系方向 + CachedContactDirX.store(Dx / DNorm); + CachedContactDirY.store(-Dy / DNorm); + CachedContactDirZ.store(Dz / DNorm); + } + else { + CachedContactDirX.store(0.0); + CachedContactDirY.store(0.0); + CachedContactDirZ.store(0.0); + } + CachedFlowX.store(OceanCurrent.LastVelocity.X); + CachedFlowY.store(-OceanCurrent.LastVelocity.Y); + CachedFlowZ.store(OceanCurrent.LastVelocity.Z); + } +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h new file mode 100644 index 0000000000..10f0fb68b6 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -0,0 +1,512 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 位姿跟随桥接。 +// +// MuJoCo 是唯一的位姿真值源,本组件每帧把 ROV 根 body 的世界位姿提供给 +// RovPawnSimApi,后者据此驱动 Pawn。AirSim 自身的 ROV 物理与 UE 侧碰撞全部关闭, +// 避免同一台 ROV 被两套物理重复积分。 +// +// 位姿直接读 MuJoCo 的 xpos / xquat 数组,不依赖 UMjBody 的 Tick 顺序, +// 因此不需要 AddTickPrerequisiteComponent。 + +#pragma once + +#include "CoreMinimal.h" +#include "Components/ActorComponent.h" +#include "Vehicles/Rov/RovUnderwaterPhysics.h" +#include "Vehicles/Rov/RovFluidParams.h" +#include +#include "RovMuJoCoBridge.generated.h" + +class AMjArticulation; +class UMjBody; + +/** 实时状态快照,供 HUD 与外部读取。 */ +USTRUCT(BlueprintType) +struct FRovStatusSnapshot +{ + GENERATED_BODY() + + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float RollDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float PitchDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float YawDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float DepthM = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float ThrustN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float ContactForceN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float CurrentSpeedMps = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector VelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector ContactPoint = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector ContactForceDir = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector FlowVelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + bool bValid = false; +}; + +// MuJoCo 的 mjModel / mjData 是 struct mjModel_ / struct mjData_ 的 typedef。 +// 这里只前向声明,避免把 mujoco.h 拉进 AirSim 的公共头。 +struct mjModel_; +struct mjData_; + +/** + * 让 AirSim ROV Pawn 跟随 MuJoCo 模型的桥接组件。 + * + * 挂在 ARovPawn 上(构造函数里自动创建)。BeginPlay 时: + * 1. 找到场景里的 AMjArticulation + * 2. 按后缀匹配找到目标 body(默认 "rov") + * 3. 关闭 Pawn 自身的物理模拟与碰撞 + * 4. 注册全局位姿源,供 RovPawnSimApi 每帧查询 + */ +UCLASS(ClassGroup = (AirSim), meta = (BlueprintSpawnableComponent)) +class AIRSIM_API URovMuJoCoBridge : public UActorComponent +{ + GENERATED_BODY() + +public: + URovMuJoCoBridge(); + + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void TickComponent(float DeltaTime, ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) override; + + /** 总开关。关掉则完全退回 AirSim 原生物理(改造前的行为)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bEnableBridge = true; + + /** + * 目标 body 名的后缀。 + * URLab 会给 spec 元素名加蓝图前缀(如 ImportTestArt_xxx_C_0_rov), + * 所以用后缀匹配而不是全等匹配。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + FString TargetBodyNameSuffix = TEXT("rov"); + + /** 是否在 BeginPlay 关闭 Pawn 的物理模拟与碰撞(避免与 MuJoCo 双计)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bDisablePawnPhysics = true; + + /** + * 是否自动启动 MuJoCo 仿真。 + * UMjPhysicsEngine 默认暂停,仿真不跑 Pawn 就不会动,因此桥接激活时自动取消暂停。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bAutoStartSimulation = true; + + /** + * MuJoCo 世界坐标到 UE 世界坐标的平移偏移(cm)。 + * Pawn 位姿每帧由 MuJoCo 覆盖,摆放位置需要通过该偏移调整。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + FVector WorldOffset = FVector::ZeroVector; + + /** 键盘推进:W/S 前后、A/D 左右、Q/E 上浮下潜。键盘只给出推力比例。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + bool bEnableKeyboardControl = true; + + /** 推进器最大推力占自重的比例。0.5 = 最大推力只有一半重力。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + float ThrusterMaxRatio = 0.5f; + + /** + * 水面高度(米,MuJoCo 模型单位)。 + * MJCF 为米制,桥接输出到 UE 时 ×100 转厘米。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + float WaterSurfaceZ = -2.0f; // 地图海面 Z = -200 cm + + // ---------------------------- 流体 ---------------------------- + + /** 是否启用 MuJoCo 原生椭球流体模型(阻力/升力交给引擎算)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseNativeFluid = true; + + /** 流体密度(kg/m³)。1000 = 水。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float FluidDensity = 1000.0f; + + /** 流体动力粘度(Pa·s)。水约 0.00089。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float FluidViscosity = 0.00089f; + + // ---------------------------- 浮力 ---------------------------- + + /** 排水体积(m³),用于 Fb = ρ·g·V。默认 0.192 与 mujoco_plugin 的 config 一致。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float DisplacedVolume = 0.192f; + + /** + * 浮力倍率,1.0 = 严格按 ρ·g·V。 + * 按 0.192 m³ 排水、整机 460.5 kg 配平,中性约需 2.34。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyScale = 2.34f; + + /** + * 机械臂排水体积(升)。 + * UR5e 连杆 11.73 L + 2F85 约 0.35 L,取 12.1 L;浮力作用在臂的合成质心。 + * 设为 0 可关闭。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float ArmBuoyancyVolume = 12.1f; + + /** 机械臂浮力系数,1.0 = 完全按真实排水体积;0.9 表示臂仍有 10% 净重。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float ArmBuoyancyScale = 1.0f; + + /** + * 浮心相对重心的高度(m),沿机体系 +Z。 + * 倾斜时浮力产生回复力矩 τ = r_CB × F_buoy,回复刚度 ≈ B × BG。 + * 设为 0 则浮力加在质心,无被动稳定。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyArm = 0.02f; + + /** + * 机械臂关节阻尼(N·m·s/rad),施加到所有非自由关节。 + * 关节无 ctrl 输入时会自由摆动,反作用力矩会持续激励艇体。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointDamping = 20000.0f; + + /** + * 机械臂关节刚度(N·m/rad),相当于在每个关节上加位置伺服弹簧, + * 把关节锁在 MJCF 的 qpos0 上。只加阻尼时机械臂仍会在重力下缓慢下垂, + * 偏置姿态产生恒定扰动力矩。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointStiffness = 0.0f; + + /** + * 机械臂重力补偿:把机械臂各 body 的 gravcomp 设为 1.0,抵消其自重。 + * 保留惯量与碰撞。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bArmGravityComp = false; + + // ---------------------------- 姿态控制 ---------------------------- + + /** + * 姿态保持:roll/pitch 回水平 + yaw 锁航向。 + * + * 分工按真实 ROV:被动稳定(浮心高于重心)负责 roll/pitch 的刚度, + * 本控制器只补阻尼和积分 —— 积分是必需的,因为机械臂挂在一侧会产生 + * 恒定扰动力矩,纯 PD 只能平衡在某个固定倾角上。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + bool bEnableAttitudeHold = true; + + /** 姿态闭环带宽(rad/s)。增益按 I·ω² 自动缩放,换惯量不用重调。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeBandwidth = 1.0f; + + /** 阻尼比。1.0 = 临界阻尼,不超调。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeZeta = 0.8f; + + /** + * 姿态积分增益(rad/s per rad·s)。消除恒定力矩造成的固定倾角, + * 积分值限幅 ±2.0 rad/s;设 0 退回纯 P。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeIntegralGain = 0.5f; + + /** 积分增益系数(相对 I·ω³)。用于消除恒定扰动造成的稳态误差。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeIntegralFactor = 0.0f; + + /** 目标航向(度)。启动时自动捕获当前航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TargetYawDeg = 0.0f; + + /** 航向调整速率(度/秒)。按住 ← / → 连续改变目标航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawRateDegPerSec = 60.0f; + + /** + * 机械臂静力矩配平(机体系,N·m),常值前馈。 + * 臂的重力矩随俯仰角变化,常值前馈无法完全抵消,默认 0。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TrimTorquePitch = 0.0f; + + /** 横滚方向配平力矩(N·m),用法同 TrimTorquePitch。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TrimTorqueRoll = 0.0f; + /** 水平(roll/pitch)回调带宽 rad/s */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float LevelBandwidth = 1.5f; + + /** 水平阻尼比 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float LevelZeta = 0.8f; + + /** 航向回调带宽 rad/s */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawBandwidth = 1.0f; + + /** 航向阻尼比 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawZeta = 1.0f; + + // ---------------------------- 洋流 ---------------------------- + + /** 洋流拖曳系数(kg/s)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + float CurrentDragCoeff = 10.0f; + + /** + * 附加角阻尼(1/s),与流体模型叠加,作为独立受力项。 + * 默认 0,旋转阻尼由 AngularDofDamping 提供。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float AngularDampingCoeff = 0.0f; + + /** + * 转动自由度阻尼(N·m·s/rad),写入 MuJoCo 的 dof_damping[3..5]。 + * 由 spin-down 实验辨识得到;<= 0 时退回「0.5 × 平均惯量」。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float AngularDofDamping = 13.0f; + + /** 是否启用洋流(三层级联:Gauss-Markov + 分层 + 湍流)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + bool bEnableOceanCurrent = true; + + /** 洋流平均流速(m/s)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + float OceanCurrentMeanSpeed = 0.15f; + + // ---------------------------- 定深 ---------------------------- + + /** PID 定深是否可用(按 Z 键切换)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Depth") + bool bEnableDepthHold = true; + + // ---------------------------- 配置文件 ---------------------------- + + /** + * 是否从配置文件加载参数。 + * 默认读取 /Config/RovParams.json,可用 -RovConfig=<路径> 指定其他文件。 + * 优先级:命令行 > 配置文件 > 类默认值。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Config") + bool bLoadConfigFile = true; + + /** 生效的配置文件绝对路径(加载后回填,便于日志核对)。 */ + UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "ROV|MuJoCo|Config") + FString LoadedConfigPath; + + // ---------------------------- 接触日志 ---------------------------- + + /** 是否把逐接触点数据写入 CSV。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log") + bool bLogContacts = false; + + /** 接触日志路径。留空则写入 /Saved/RovContactLog.csv。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log") + FString ContactLogPath; + + /** 接触日志降采样间隔(物理步数)。10 = 500 Hz 步长下 50 Hz。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log", meta = (ClampMin = "1")) + int32 ContactLogInterval = 10; + + // ---------------------------- 显式阻力与附加质量 ---------------------------- + + /** + * 是否叠加显式二次阻力 F = 0.5·ρ·Cd·A·v²(沿机体系三轴)。 + * 与 MuJoCo 原生椭球模型并联,用于补偿椭球近似对大钝体/细长体的偏差。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseExplicitDrag = false; + + /** 机体系三轴阻力系数 Cd。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector DragCoeff = FVector(0.82f, 1.15f, 1.05f); + + /** 机体系三轴迎流参考面积(m²)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector DragRefArea = FVector(0.24f, 0.32f, 0.48f); + + /** + * 是否叠加附加质量力 F = -m_A·a(沿机体系三轴)。 + * MuJoCo 椭球模型内部已含附加质量,本项用于在不改模型的前提下单独调整。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseAddedMass = false; + + /** 机体系三轴附加质量(kg)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector AddedMass = FVector(0.0f, 0.0f, 0.0f); + + /** 按外形自动估算并覆盖 DragCoeff / DragRefArea / AddedMass。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bAutoEstimateFluidParams = false; + + /** 自动估算用的外形。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + ERovHullShape HullShape = ERovHullShape::Box; + + /** 自动估算用的外形半尺寸(m)。含义见 URovFluidLibrary::EstimateFluidParams。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector HullDims = FVector(0.4f, 0.3f, 0.2f); + + /** 读取实时状态快照(姿态、深度、推力、接触力、洋流)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|MuJoCo") + void GetStatusSnapshot(FRovStatusSnapshot& Out) const; + + /** 桥接是否已生效。 */ + UFUNCTION(BlueprintCallable, BlueprintPure, Category = "ROV|MuJoCo") + bool IsBridgeActive() const { return bBridgeActive; } + + /** 当前跟随的 body 名(调试用)。 */ + UFUNCTION(BlueprintCallable, BlueprintPure, Category = "ROV|MuJoCo") + FString GetResolvedBodyName() const; + + /** + * 全局位姿源。RovPawnSimApi 每帧调用它取位姿。 + * 给出的是 UE 世界坐标;AirSim 侧的 NED 转换由 PawnSimApi::toPoseFromUnreal 完成。 + * 为空表示没有外部源,RovPawnSimApi 退回读取自身物理体。 + */ + static std::function ExternalPoseProvider; + +private: + /** 供 ExternalPoseProvider 调用的实际取值函数。 */ + bool ProvidePose(FVector& OutPosition, FQuat& OutRotation) const; + + /** + * 解析 AMjArticulation 与目标 UMjBody。 + * 模型通常在 Pawn 生成之后才编译完成,因此声明为 const + mutable, + * 由 ProvidePose 在运行中惰性重试。 + */ + void ResolveTargets() const; + + /** 关闭 Pawn 物理与碰撞,避免与 MuJoCo 重复计算。 */ + void DisablePawnPhysics(); + + /** 读取配置文件并覆盖对应参数(命令行稍后覆盖,优先级最低)。 */ + void ApplyConfigFile(); + + /** + * 级联姿态控制(物理线程,500 Hz)。 + * 输出机体系力矩;积分步长取物理步长,因此与渲染帧率无关。 + */ + void ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int32 BodyId, double Dt, double OutTorque[3]); + + /** 逐接触点写 CSV(4.2.4)。 */ + void LogContacts(mjModel_* Model, mjData_* Data); + + /** 每帧把键盘输入换算成推力与力矩,写入缓存。 */ + void ApplyControlForce(); + + /** + * 物理线程内施力:从缓存读出推力/力矩并写入 MuJoCo。 + * 通过 UMjPhysicsEngine 的 PreStepCallbacks 在 mj_step 之前执行, + * 与物理步同锁,避免被 mj_step 覆盖。 + */ + void ApplyCachedForce(mjModel_* Model, mjData_* Data); + + /** 期望推力(N),游戏线程写、物理线程读。 */ + std::atomic CachedForceX{ 0.0 }; + std::atomic CachedForceY{ 0.0 }; + std::atomic CachedForceZ{ 0.0 }; + + /** 期望力矩(N·m),**世界系**(mj_applyFT 的入参就是世界系)。 */ + std::atomic CachedTorqueX{ 0.0 }; + std::atomic CachedTorqueY{ 0.0 }; + std::atomic CachedTorqueZ{ 0.0 }; + + /** 浮力大小(N)。单独存,因为要作用在浮心而不是质心。 */ + std::atomic CachedBuoyancy{ 0.0 }; + + std::atomic CachedBodyId{ -1 }; + + /** + * 控制指令时间戳(MuJoCo 仿真时间)。 + * 指令在游戏线程按帧更新、物理在独立线程按 500 Hz 步进; + * 物理线程据此判断指令是否过期,过期时把推力与力矩归零。 + */ + std::atomic CachedControlTime{ 0.0 }; + + /** pre-step 回调只注册一次。 */ + bool bControlCallbackRegistered = false; + + /** 洋流模型(三层级联)。 */ + FRovOceanCurrent OceanCurrent; + + /** PID 定深控制器。 */ + FRovPIDDepthController DepthPID; + + /** 是否正在定深。 */ + bool bDepthHoldActive = false; + + /** 定深目标(MuJoCo 世界系 Z,m)。 */ + double DepthHoldTargetZ = 0.0; + + /** Z 键上一帧状态,用于边沿检测。 */ + bool bPrevDepthKeyDown = false; + + /** 姿态控制器积分项(机体系,rad·s)。 */ + FVector AttitudeIntegral = FVector::ZeroVector; + + /** 物理线程姿态环的积分状态(与帧率无关)。 */ + double AttIntState[3] = { 0.0, 0.0, 0.0 }; + + /** 航向角速度前馈(rad/s,机体系),游戏线程写、物理线程读。 */ + std::atomic CachedYawFFRadPerSec{ 0.0 }; + + /** 姿态控制器是否已初始化(首次捕获目标航向)。 */ + bool bAttitudeInitialized = false; + + /** + * 自由关节 6×6 质量阵的转动对角元(kg·m²),即 ROV+机械臂的**复合惯量**。 + * body_inertia 只有 ROV 本体,不含机械臂;用它算增益会严重偏小。 + */ + double EffInertia[3] = { 0.0, 0.0, 0.0 }; + + mutable TWeakObjectPtr Articulation; + mutable TWeakObjectPtr TargetBody; + + /** 只在第一次解析失败时打印候选列表,避免每帧刷屏。 */ + mutable bool bResolveWarned = false; + + /** 接触日志文件句柄(EndPlay 时关闭)。 */ + FArchive* ContactLogArchive = nullptr; + int64 ContactLogStep = 0; + bool bContactLogHeaderWritten = false; + + /** 实时状态缓存,物理线程写、游戏线程读。 */ + std::atomic CachedContactForce{ 0.0 }; + std::atomic CachedContactPointX{ 0.0 }; + std::atomic CachedContactPointY{ 0.0 }; + std::atomic CachedContactPointZ{ 0.0 }; + std::atomic CachedContactDirX{ 0.0 }; + std::atomic CachedContactDirY{ 0.0 }; + std::atomic CachedContactDirZ{ 0.0 }; + std::atomic CachedVelocityX{ 0.0 }; + std::atomic CachedVelocityY{ 0.0 }; + std::atomic CachedVelocityZ{ 0.0 }; + std::atomic CachedFlowX{ 0.0 }; + std::atomic CachedFlowY{ 0.0 }; + std::atomic CachedFlowZ{ 0.0 }; + std::atomic CachedRollDeg{ 0.0 }; + std::atomic CachedPitchDeg{ 0.0 }; + std::atomic CachedYawDeg{ 0.0 }; + std::atomic CachedDepthM{ 0.0 }; + std::atomic CachedThrustN{ 0.0 }; + + bool bBridgeActive = false; + + /** 调试用:-RovContactTest 时施加的额外下压力(N),把 ROV 压到海床以产生接触。 */ + double ContactTestForce = 0.0; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp new file mode 100644 index 0000000000..d5611cb55c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp @@ -0,0 +1,121 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovObstacleField.h" + +#include "Components/StaticMeshComponent.h" +#include "Components/SceneComponent.h" +#include "Engine/StaticMesh.h" +#include "Materials/MaterialInstanceDynamic.h" +#include "UObject/ConstructorHelpers.h" + +ARovObstacleField::ARovObstacleField() +{ + PrimaryActorTick.bCanEverTick = false; + RootComponent = CreateDefaultSubobject(TEXT("Root")); +} + +void ARovObstacleField::BuildDefaults() +{ + Obstacles.Reset(); + + // 位置与 MJCF 里的障碍几何体一一对应(相对 ROV 出生点,单位 cm)。 + // 换算:UE = 100 * (x, -y, z - z_rov),z_rov 取出生高度 -1.0 m。 + // 圆柱默认轴为 Z:轴向沿 X 用 Pitch=90,沿 Y 用 Pitch=90 + Yaw=90。 + + // 1. 海底主干管道与分支管道 + Obstacles.Add({ ERovObstacleType::Pipe, FVector(0.0f, -320.0f, -160.0f), FVector(25.0f, 25.0f, 450.0f), FRotator(90.0f, 0.0f, 0.0f) }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(300.0f, -100.0f, -160.0f), FVector(20.0f, 20.0f, 300.0f), FRotator(90.0f, 90.0f, 0.0f) }); + + // 2. 井口采油树(基座 + 立管) + Obstacles.Add({ ERovObstacleType::Frame, FVector(180.0f, 50.0f, -200.0f), FVector(60.0f, 60.0f, 50.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(180.0f, 50.0f, -90.0f), FVector(22.0f, 22.0f, 60.0f), FRotator::ZeroRotator }); + + // 3. 导管架支撑桩(两根立柱 + 横梁) + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, -180.0f, -80.0f), FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 180.0f, -80.0f), FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 0.0f, -200.0f), FVector(12.0f, 12.0f, 180.0f), FRotator(90.0f, 90.0f, 0.0f) }); + + // 4. 检修结构体与礁石 + Obstacles.Add({ ERovObstacleType::Frame, FVector(0.0f, 320.0f, -140.0f), FVector(50.0f, 80.0f, 60.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Reef, FVector(-50.0f, -150.0f, -210.0f), FVector(40.0f, 40.0f, 40.0f), FRotator::ZeroRotator }); +} +void ARovObstacleField::OnConstruction(const FTransform& Transform) +{ + Super::OnConstruction(Transform); + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) { + BuildDefaults(); + } +} + +void ARovObstacleField::BeginPlay() +{ + Super::BeginPlay(); + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) { + BuildDefaults(); + } + Rebuild(); +} + +void ARovObstacleField::Rebuild() +{ + for (UStaticMeshComponent* Comp : SpawnedComponents) { + if (Comp) { + Comp->DestroyComponent(); + } + } + SpawnedComponents.Reset(); + + const FVector Base = AnchorActor ? AnchorActor->GetActorLocation() : GetActorLocation(); + + UStaticMesh* MeshPipe = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Cylinder.Cylinder")); + UStaticMesh* MeshBox = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Cube.Cube")); + UStaticMesh* MeshSphere = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Sphere.Sphere")); + + // 引擎基础网格的原始尺寸:圆柱 r=50 h=100,立方体 100³,球 r=50(单位 cm) + for (const FRovObstacleDef& Def : Obstacles) { + UStaticMesh* Mesh = nullptr; + FVector Scale = FVector::OneVector; + FLinearColor Color(0.6f, 0.6f, 0.6f); + + switch (Def.Type) { + case ERovObstacleType::Pipe: + Mesh = MeshPipe; + Scale = FVector(Def.Size.X / 50.0f, Def.Size.Y / 50.0f, Def.Size.Z / 50.0f); + Color = FLinearColor(0.55f, 0.42f, 0.20f); + break; + case ERovObstacleType::Frame: + Mesh = MeshBox; + Scale = Def.Size / 50.0f; + Color = FLinearColor(0.35f, 0.42f, 0.50f); + break; + case ERovObstacleType::Reef: + Mesh = MeshSphere; + Scale = Def.Size / 50.0f; + Color = FLinearColor(0.45f, 0.38f, 0.30f); + break; + default: + break; + } + + if (!Mesh) { + continue; + } + + UStaticMeshComponent* Comp = NewObject(this); + Comp->SetStaticMesh(Mesh); + Comp->SetupAttachment(RootComponent); + Comp->RegisterComponent(); + Comp->SetWorldLocation(Base + Def.Location); + Comp->SetWorldRotation(Def.Rotation); + Comp->SetWorldScale3D(Scale); + Comp->SetCollisionEnabled(ECollisionEnabled::NoCollision); + Comp->SetCastShadow(false); + + if (UMaterialInstanceDynamic* MID = Comp->CreateAndSetMaterialInstanceDynamic(0)) { + MID->SetVectorParameterValue(TEXT("Color"), Color); + } + SpawnedComponents.Add(Comp); + } + + UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物 %d 个(管道/框架/礁石),锚点 %s"), SpawnedComponents.Num(), *Base.ToString()); +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h new file mode 100644 index 0000000000..8477cf6f78 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h @@ -0,0 +1,81 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// 障碍物碰撞场景:管道、框架、礁石三类障碍的运行时摆放。 + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "RovObstacleField.generated.h" + +class UStaticMeshComponent; + +/** 障碍类型。 */ +UENUM(BlueprintType) +enum class ERovObstacleType : uint8 +{ + Pipe UMETA(DisplayName = "管道"), + Frame UMETA(DisplayName = "框架"), + Reef UMETA(DisplayName = "礁石"), +}; + +/** 单个障碍定义。位置相对锚点(通常是 ROV 出生点),单位 cm。 */ +USTRUCT(BlueprintType) +struct FRovObstacleDef +{ + GENERATED_BODY() + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + ERovObstacleType Type = ERovObstacleType::Pipe; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FVector Location = FVector::ZeroVector; + + /** 尺寸(cm):圆柱为 (半径, 半径, 半高),立方体为半尺寸,球为半径。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FVector Size = FVector(50.0f, 50.0f, 200.0f); + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FRotator Rotation = FRotator::ZeroRotator; +}; + +/** + * 障碍物场。挂在关卡里或由 ROV 仿真模式自动生成。 + * + * 默认摆放:3 段管道、4 根框架立柱、3 块礁石,围绕锚点分布, + * 与 MuJoCo 模型里的 pipeline_* / pile_* / reef_* 碰撞体一一对应。 + * 这些障碍直接复用给 4.1.3 / 7.2.x 的接近与碰撞试验。 + */ +UCLASS() +class AIRSIM_API ARovObstacleField : public AActor +{ + GENERATED_BODY() + +public: + ARovObstacleField(); + + virtual void OnConstruction(const FTransform& Transform) override; + virtual void BeginPlay() override; + + /** 障碍列表。为空且 bSpawnDefaultObstacles=true 时自动生成默认三类障碍。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + TArray Obstacles; + + /** 未显式配置时是否生成默认障碍。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + bool bSpawnDefaultObstacles = true; + + /** 摆放锚点。为空则以自身位置为锚点。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + AActor* AnchorActor = nullptr; + + /** 重建全部障碍(改配置后调用)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Obstacle") + void Rebuild(); + +private: + void BuildDefaults(); + + UPROPERTY() + TArray SpawnedComponents; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp new file mode 100644 index 0000000000..6d47d9f35d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -0,0 +1,280 @@ +#include "RovPawn.h" +#include "Components/StaticMeshComponent.h" +#include "Components/ChildActorComponent.h" +#include "UObject/ConstructorHelpers.h" +#include "AirBlueprintLib.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" +#include "common/CommonStructs.hpp" +#include "common/Common.hpp" + +ARovPawn::ARovPawn() +{ + AutoPossessPlayer = EAutoReceiveInput::Player0; + init_id_ = pawn_events_.getActuatorSignal().connect_member(this, &ARovPawn::setRotorRenderedStates); + + RootComponent = CreateDefaultSubobject(TEXT("RootComponent")); + + // MuJoCo 位姿跟随桥接 + MuJoCoBridge_ = CreateDefaultSubobject(TEXT("MuJoCoBridge")); + + body_mesh_ = CreateDefaultSubobject(TEXT("BodyMesh")); + body_mesh_->SetupAttachment(RootComponent); + + static ConstructorHelpers::FObjectFinder cube_mesh(TEXT("/Engine/BasicShapes/Cube.Cube")); + if (cube_mesh.Succeeded()) { + body_mesh_->SetStaticMesh(cube_mesh.Object); + body_mesh_->SetRelativeScale3D(FVector(0.5f, 0.35f, 0.25f)); + } + + static ConstructorHelpers::FClassFinder pip_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); + UClass* pip_class = pip_camera_class.Succeeded() ? pip_camera_class.Class : nullptr; + + camera_front_center_comp_ = CreateDefaultSubobject(TEXT("FrontCenterCamera")); + camera_front_center_comp_->SetupAttachment(RootComponent); + camera_front_center_comp_->SetRelativeLocation(FVector(25.f, 0.f, 0.f)); + if (pip_class) camera_front_center_comp_->SetChildActorClass(pip_class); + + camera_front_right_comp_ = CreateDefaultSubobject(TEXT("FrontRightCamera")); + camera_front_right_comp_->SetupAttachment(RootComponent); + camera_front_right_comp_->SetRelativeLocation(FVector(25.f, 15.f, 0.f)); + if (pip_class) camera_front_right_comp_->SetChildActorClass(pip_class); + + camera_front_left_comp_ = CreateDefaultSubobject(TEXT("FrontLeftCamera")); + camera_front_left_comp_->SetupAttachment(RootComponent); + camera_front_left_comp_->SetRelativeLocation(FVector(25.f, -15.f, 0.f)); + if (pip_class) camera_front_left_comp_->SetChildActorClass(pip_class); + + camera_back_center_comp_ = CreateDefaultSubobject(TEXT("BackCenterCamera")); + camera_back_center_comp_->SetupAttachment(RootComponent); + camera_back_center_comp_->SetRelativeLocation(FVector(-25.f, 0.f, 0.f)); + if (pip_class) camera_back_center_comp_->SetChildActorClass(pip_class); + + camera_bottom_center_comp_ = CreateDefaultSubobject(TEXT("BottomCenterCamera")); + camera_bottom_center_comp_->SetupAttachment(RootComponent); + camera_bottom_center_comp_->SetRelativeLocation(FVector(0.f, 0.f, -15.f)); + camera_bottom_center_comp_->SetRelativeRotation(FRotator(-90.f, 0.f, 0.f)); + if (pip_class) camera_bottom_center_comp_->SetChildActorClass(pip_class); +} + +void ARovPawn::BeginPlay() +{ + Super::BeginPlay(); + + if (body_mesh_) { + const TCHAR* candidates[] = { + TEXT("/AirSim/Models/RoV/bluerovheavy2.bluerovheavy2"), + TEXT("/AirSim/Models/RoV/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001"), + TEXT("/Game/bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001.bluerovheavy2_BROV2-HEAVY-ASM-BROV2-HEAVY-R1_001"), + TEXT("/Game/bluerovheavy2.bluerovheavy2") + }; + UStaticMesh* rov_mesh = nullptr; + for (const TCHAR* candidate : candidates) { + rov_mesh = Cast(StaticLoadObject(UStaticMesh::StaticClass(), nullptr, candidate)); + if (rov_mesh) { + UE_LOG(LogTemp, Log, TEXT("ARovPawn: Loaded ROV mesh from %s"), candidate); + break; + } + } + if (rov_mesh) { + body_mesh_->SetStaticMesh(rov_mesh); + + FBoxSphereBounds bounds = rov_mesh->GetBounds(); + FVector extent = bounds.BoxExtent; + + UE_LOG(LogTemp, Warning, TEXT("ARovPawn ROV Mesh Bounds: Origin=(%f, %f, %f), Extent=(%f, %f, %f)"), bounds.Origin.X, bounds.Origin.Y, bounds.Origin.Z, extent.X, extent.Y, extent.Z); + + float max_dim = FMath::Max3(extent.X, extent.Y, extent.Z) * 2.0f; + float target_size = 50.0f; // BlueROV2 is approx 50cm + float scale = 1.0f; + if (max_dim > 100.0f) { + scale = target_size / max_dim; + } + else if (max_dim < 5.0f && max_dim > 0.001f) { + scale = target_size / max_dim; + } + + body_mesh_->SetRelativeScale3D(FVector(scale, scale, scale)); + body_mesh_->SetRelativeLocation(-bounds.Origin * scale); + } + } + + rotor_speed_components_.Empty(); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV1"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV2"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV3"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationV4"))); + + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH1"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH2"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH3"))); + rotor_speed_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("RotationH4"))); + + rotor_angle_components_.Empty(); + rotor_angle_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("Engine_L"))); + rotor_angle_components_.Add(UAirBlueprintLib::GetActorComponent(this, TEXT("Engine_R"))); +} + +void ARovPawn::initializeForBeginPlay() +{ + //get references of existing camera safely + auto c_fr = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontRightCamera")); + camera_front_right_ = c_fr ? Cast(c_fr->GetChildActor()) : nullptr; + + auto c_fl = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontLeftCamera")); + camera_front_left_ = c_fl ? Cast(c_fl->GetChildActor()) : nullptr; + + auto c_fc = UAirBlueprintLib::GetActorComponent(this, TEXT("FrontCenterCamera")); + camera_front_center_ = c_fc ? Cast(c_fc->GetChildActor()) : nullptr; + + auto c_bc = UAirBlueprintLib::GetActorComponent(this, TEXT("BackCenterCamera")); + camera_back_center_ = c_bc ? Cast(c_bc->GetChildActor()) : nullptr; + + auto c_bot = UAirBlueprintLib::GetActorComponent(this, TEXT("BottomCenterCamera")); + camera_bottom_center_ = c_bot ? Cast(c_bot->GetChildActor()) : nullptr; +} + +void ARovPawn::SetupPlayerInputComponent(UInputComponent* PlayerInputComponent) +{ + Super::SetupPlayerInputComponent(PlayerInputComponent); + if (!PlayerInputComponent) return; + + PlayerInputComponent->BindKey(EKeys::W, IE_Pressed, this, &ARovPawn::onForwardPressed); + PlayerInputComponent->BindKey(EKeys::W, IE_Released, this, &ARovPawn::onForwardReleased); + PlayerInputComponent->BindKey(EKeys::Up, IE_Pressed, this, &ARovPawn::onForwardPressed); + PlayerInputComponent->BindKey(EKeys::Up, IE_Released, this, &ARovPawn::onForwardReleased); + + PlayerInputComponent->BindKey(EKeys::S, IE_Pressed, this, &ARovPawn::onBackwardPressed); + PlayerInputComponent->BindKey(EKeys::S, IE_Released, this, &ARovPawn::onBackwardReleased); + PlayerInputComponent->BindKey(EKeys::Down, IE_Pressed, this, &ARovPawn::onBackwardPressed); + PlayerInputComponent->BindKey(EKeys::Down, IE_Released, this, &ARovPawn::onBackwardReleased); + + PlayerInputComponent->BindKey(EKeys::Q, IE_Pressed, this, &ARovPawn::onLeftPressed); + PlayerInputComponent->BindKey(EKeys::Q, IE_Released, this, &ARovPawn::onLeftReleased); + PlayerInputComponent->BindKey(EKeys::E, IE_Pressed, this, &ARovPawn::onRightPressed); + PlayerInputComponent->BindKey(EKeys::E, IE_Released, this, &ARovPawn::onRightReleased); + + PlayerInputComponent->BindKey(EKeys::A, IE_Pressed, this, &ARovPawn::onYawLeftPressed); + PlayerInputComponent->BindKey(EKeys::A, IE_Released, this, &ARovPawn::onYawLeftReleased); + PlayerInputComponent->BindKey(EKeys::Left, IE_Pressed, this, &ARovPawn::onYawLeftPressed); + PlayerInputComponent->BindKey(EKeys::Left, IE_Released, this, &ARovPawn::onYawLeftReleased); + + PlayerInputComponent->BindKey(EKeys::D, IE_Pressed, this, &ARovPawn::onYawRightPressed); + PlayerInputComponent->BindKey(EKeys::D, IE_Released, this, &ARovPawn::onYawRightReleased); + PlayerInputComponent->BindKey(EKeys::Right, IE_Pressed, this, &ARovPawn::onYawRightPressed); + PlayerInputComponent->BindKey(EKeys::Right, IE_Released, this, &ARovPawn::onYawRightReleased); + + PlayerInputComponent->BindKey(EKeys::SpaceBar, IE_Pressed, this, &ARovPawn::onUpPressed); + PlayerInputComponent->BindKey(EKeys::SpaceBar, IE_Released, this, &ARovPawn::onUpReleased); + PlayerInputComponent->BindKey(EKeys::PageUp, IE_Pressed, this, &ARovPawn::onUpPressed); + PlayerInputComponent->BindKey(EKeys::PageUp, IE_Released, this, &ARovPawn::onUpReleased); + + PlayerInputComponent->BindKey(EKeys::C, IE_Pressed, this, &ARovPawn::onDownPressed); + PlayerInputComponent->BindKey(EKeys::C, IE_Released, this, &ARovPawn::onDownReleased); + PlayerInputComponent->BindKey(EKeys::PageDown, IE_Pressed, this, &ARovPawn::onDownPressed); + PlayerInputComponent->BindKey(EKeys::PageDown, IE_Released, this, &ARovPawn::onDownReleased); +} + +void ARovPawn::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + pawn_events_.getPawnTickSignal().emit(DeltaSeconds); + + float fwd = (bForward_ ? 1.0f : 0.0f) - (bBackward_ ? 1.0f : 0.0f); + float lat = (bRight_ ? 1.0f : 0.0f) - (bLeft_ ? 1.0f : 0.0f); + float vert = (bDown_ ? 1.0f : 0.0f) - (bUp_ ? 1.0f : 0.0f); + float yaw = (bYawRight_ ? 1.0f : 0.0f) - (bYawLeft_ ? 1.0f : 0.0f); + + bool bHasInput = (std::abs(fwd) > 0.01f || std::abs(lat) > 0.01f || std::abs(vert) > 0.01f || std::abs(yaw) > 0.01f); + + if (bHasInput) { + std::vector pwms(8, 0.0f); + pwms[0] = vert; + pwms[1] = vert; + pwms[2] = vert; + pwms[3] = vert; + pwms[4] += fwd * 1.0f + lat * -1.0f + yaw * 1.0f; + pwms[5] += fwd * 1.0f + lat * 1.0f + yaw * -1.0f; + pwms[6] += fwd * -1.0f + lat * 1.0f + yaw * -1.0f; + pwms[7] += fwd * -1.0f + lat * -1.0f + yaw * 1.0f; + + for (int i = 0; i < 8; ++i) { + pwms[i] = FMath::Clamp(pwms[i], -1.0f, 1.0f); + } + + pawn_events_.getKeyboardPwmSignal().emit(pwms); + bHadInput_ = true; + } + else if (bHadInput_) { + std::vector zero_pwms(8, 0.0f); + pawn_events_.getKeyboardPwmSignal().emit(zero_pwms); + bHadInput_ = false; + } +} + +void ARovPawn::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + camera_front_right_ = nullptr; + camera_front_left_ = nullptr; + camera_front_center_ = nullptr; + camera_back_center_ = nullptr; + camera_bottom_center_ = nullptr; + + pawn_events_.getActuatorSignal().disconnect_all(); + rotor_speed_components_.Empty(); + rotor_angle_components_.Empty(); + + Super::EndPlay(EndPlayReason); +} + +const common_utils::UniqueValueMap ARovPawn::getCameras() const +{ + common_utils::UniqueValueMap cameras; + cameras.insert_or_assign("front_center", camera_front_center_); + cameras.insert_or_assign("front_right", camera_front_right_); + cameras.insert_or_assign("front_left", camera_front_left_); + cameras.insert_or_assign("bottom_center", camera_bottom_center_); + cameras.insert_or_assign("back_center", camera_back_center_); + + cameras.insert_or_assign("0", camera_front_center_); + cameras.insert_or_assign("1", camera_front_right_); + cameras.insert_or_assign("2", camera_front_left_); + cameras.insert_or_assign("3", camera_bottom_center_); + cameras.insert_or_assign("4", camera_back_center_); + + cameras.insert_or_assign("", camera_front_center_); + cameras.insert_or_assign("fpv", camera_front_center_); + + return cameras; +} + +void ARovPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, + FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) +{ + pawn_events_.getCollisionSignal().emit(MyComp, Other, OtherComp, bSelfMoved, HitLocation, HitNormal, NormalImpulse, Hit); +} + +void ARovPawn::setRotorRenderedStates(const std::vector& rotor_infos) +{ + int info_count = static_cast(rotor_infos.size()); + int max_rotors = FMath::Min(num_visible_rotors_, info_count); + + for (auto rotor_index = 0; rotor_index < max_rotors; ++rotor_index) { + if (rotor_index < rotor_speed_components_.Num()) { + URotatingMovementComponent* rot_movement = rotor_speed_components_[rotor_index]; + if (rot_movement != nullptr) { + rot_movement->RotationRate.Yaw = + rotor_infos.at(rotor_index).rotor_speed * rotor_infos.at(rotor_index).rotor_direction * RotatorFactor; + } + } + if (!rotor_infos.at(rotor_index).is_fixed) { + if (rotor_index < rotor_angle_components_.Num()) { + UStaticMeshComponent* rotor_angle_comp = rotor_angle_components_[rotor_index]; + if (rotor_angle_comp != nullptr) { + float pitch = -rotor_infos.at(rotor_index).rotor_angle_from_vertical * 180.0f / M_PIf; // negate for unreal axes + FRotator new_rotation = FRotator(pitch, 0.0f, 0.0f); + rotor_angle_comp->SetRelativeRotation(new_rotation); + } + } + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h new file mode 100644 index 0000000000..44646ac9fb --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h @@ -0,0 +1,110 @@ +#pragma once + +#include "GameFramework/RotatingMovementComponent.h" + +#include +#include "PIPCamera.h" +#include "common/common_utils/Signal.hpp" +#include "common/common_utils/UniqueValueMap.hpp" +#include "RovPawnEvents.h" + +#include "RovPawn.generated.h" + +UCLASS() +class AIRSIM_API ARovPawn : public APawn +{ + GENERATED_BODY() + +public: + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Debugging") + float RotatorFactor = 1.3f; + + ARovPawn(); + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, + FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) override; + virtual void SetupPlayerInputComponent(class UInputComponent* PlayerInputComponent) override; + + void onForwardPressed() { bForward_ = true; } + void onForwardReleased() { bForward_ = false; } + void onBackwardPressed() { bBackward_ = true; } + void onBackwardReleased() { bBackward_ = false; } + void onLeftPressed() { bLeft_ = true; } + void onLeftReleased() { bLeft_ = false; } + void onRightPressed() { bRight_ = true; } + void onRightReleased() { bRight_ = false; } + void onUpPressed() { bUp_ = true; } + void onUpReleased() { bUp_ = false; } + void onDownPressed() { bDown_ = true; } + void onDownReleased() { bDown_ = false; } + void onYawLeftPressed() { bYawLeft_ = true; } + void onYawLeftReleased() { bYawLeft_ = false; } + void onYawRightPressed() { bYawRight_ = true; } + void onYawRightReleased() { bYawRight_ = false; } + + //interface + void initializeForBeginPlay(); + const common_utils::UniqueValueMap getCameras() const; + RovPawnEvents* getPawnEvents() + { + return &pawn_events_; + } + //called by API to set rotor speed + void setRotorRenderedStates(const std::vector& rotor_infos); + +private: //variables + UPROPERTY() + class UStaticMeshComponent* body_mesh_; + + // MuJoCo 位姿跟随桥接组件 + UPROPERTY() + class URovMuJoCoBridge* MuJoCoBridge_; + + UPROPERTY() + class UChildActorComponent* camera_front_right_comp_; + UPROPERTY() + class UChildActorComponent* camera_front_left_comp_; + UPROPERTY() + class UChildActorComponent* camera_front_center_comp_; + UPROPERTY() + class UChildActorComponent* camera_back_center_comp_; + UPROPERTY() + class UChildActorComponent* camera_bottom_center_comp_; + + //Unreal components + UPROPERTY() + APIPCamera* camera_front_left_; + UPROPERTY() + APIPCamera* camera_front_right_; + UPROPERTY() + APIPCamera* camera_front_center_; + UPROPERTY() + APIPCamera* camera_back_center_; + UPROPERTY() + APIPCamera* camera_bottom_center_; + + UPROPERTY() + TArray rotor_speed_components_; + UPROPERTY() + TArray rotor_angle_components_; + + // TODO: hard-coding this value here since using tri-Rov physics with dual-Rov mesh + // back rotor is not animated; order of rotors is 0: left, 1: right, 2: back, so just iterate + // through first two rotors for visualization + const int num_visible_rotors_ = 8; + + RovPawnEvents pawn_events_; + int init_id_; + + bool bForward_ = false; + bool bBackward_ = false; + bool bLeft_ = false; + bool bRight_ = false; + bool bUp_ = false; + bool bDown_ = false; + bool bYawLeft_ = false; + bool bYawRight_ = false; + bool bHadInput_ = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp new file mode 100644 index 0000000000..d260e91adf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp @@ -0,0 +1,11 @@ +#include "RovPawnEvents.h" + +RovPawnEvents::ActuatorsSignal& RovPawnEvents::getActuatorSignal() +{ + return actuator_signal_; +} + +RovPawnEvents::KeyboardPwmSignal& RovPawnEvents::getKeyboardPwmSignal() +{ + return keyboard_pwm_signal_; +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h new file mode 100644 index 0000000000..5296eb7289 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h @@ -0,0 +1,34 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Pawn.h" + +#include "PawnEvents.h" +#include "common/Common.hpp" +#include "common/common_utils/Signal.hpp" + +class RovPawnEvents : public PawnEvents +{ +public: //types + typedef msr::airlib::real_T real_T; + struct RotorTiltableInfo + { + real_T rotor_speed = 0; + int rotor_direction = 0; + real_T rotor_thrust = 0; + real_T rotor_control_filtered = 0; + real_T rotor_angle_from_vertical = 0; + bool is_fixed = false; + }; + + typedef common_utils::Signal&> ActuatorsSignal; + typedef common_utils::Signal&> KeyboardPwmSignal; + +public: + ActuatorsSignal& getActuatorSignal(); + KeyboardPwmSignal& getKeyboardPwmSignal(); + +private: + ActuatorsSignal actuator_signal_; + KeyboardPwmSignal keyboard_pwm_signal_; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp new file mode 100644 index 0000000000..bfd0cca031 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp @@ -0,0 +1,254 @@ +#include "RovPawnSimApi.h" +#include "AirBlueprintLib.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" +#include "vehicles/rov/RovParamsFactory.hpp" +#include "UnrealSensors/UnrealSensorFactory.h" +#include + +using namespace msr::airlib; + +RovPawnSimApi::RovPawnSimApi(const Params& params) + : PawnSimApi(params), pawn_events_(static_cast(params.pawn_events)) +{ +} + +void RovPawnSimApi::initialize() +{ + PawnSimApi::initialize(); + + //create vehicle API + std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); + vehicle_params_ = RovBodyParamsFactory::createConfig(getVehicleSetting(), sensor_factory); + vehicle_api_ = vehicle_params_->createRovApi(); + //setup physics vehicle + rov_physics_body_ = std::unique_ptr(new RovPhysicsBody(vehicle_params_.get(), vehicle_api_.get(), getKinematics(), getEnvironment())); + + vehicle_api_->setSimulatedGroundTruth(getGroundTruthKinematics(), getGroundTruthEnvironment()); + vehicle_api_->setCollisionInfo(CollisionInfo()); + + //initialize private vars + last_phys_pose_ = pending_phys_pose_ = Pose::nanPose(); + pending_pose_status_ = PendingPoseStatus::NonePending; + reset_pending_ = false; + did_reset_ = false; + rotor_count_ = rov_physics_body_->rotorCount(); + rotor_states_.rotors.assign(rotor_count_, RotorTiltableParameters()); + rotor_infos_.assign(rotor_count_, RotorTiltableInfo()); + + //reset roll & pitch of vehicle as aircraft required to be on flat surface at start + Pose pose = getPose(); + float pitch, roll, yaw; + VectorMath::toEulerianAngle(pose.orientation, pitch, roll, yaw); + pose.orientation = VectorMath::toQuaternion(0, 0, yaw); + setPose(pose, false); + + pawn_events_->getKeyboardPwmSignal().connect_member(this, &RovPawnSimApi::onKeyboardPwm); +} + +void RovPawnSimApi::onKeyboardPwm(const std::vector& pwms) +{ + if (vehicle_api_) { + vehicle_api_->setMotorPWMs(pwms); + } +} + +void RovPawnSimApi::pawnTick(float dt) +{ + unused(dt); + //calls to update* are handled by physics engine and in SimModeWorldBase +} + +void RovPawnSimApi::updateRenderedState(float dt) +{ + //Utils::log("------Render tick-------"); + + //if reset is pending then do it first, no need to do other things until next tick + if (reset_pending_) { + reset_task_(); + did_reset_ = true; + return; + } + + //move collision info from rendering engine to vehicle + const CollisionInfo& collision_info = getCollisionInfo(); + rov_physics_body_->setCollisionInfo(collision_info); + + // 设置外部位姿源后本类退化为纯跟随者,不再使用 AirSim 物理体的状态; + // 坐标转换统一走基类的 toPoseFromUnreal()。 + FVector ExternalPos; + FQuat ExternalRot; + bool bHasExternalPose = false; + + // 优先用实例级 provider(测试或定制场景) + if (external_pose_provider_) { + bHasExternalPose = external_pose_provider_(ExternalPos, ExternalRot); + } + // 其次用 URovMuJoCoBridge 注册的全局 provider(正式运行时走这条) + if (!bHasExternalPose && URovMuJoCoBridge::ExternalPoseProvider) { + bHasExternalPose = URovMuJoCoBridge::ExternalPoseProvider(ExternalPos, ExternalRot); + } + + if (bHasExternalPose) { + last_phys_pose_ = toPoseFromUnreal(ExternalPos, ExternalRot); + } + else { + last_phys_pose_ = rov_physics_body_->getPose(); + } + + collision_response = rov_physics_body_->getCollisionResponseInfo(); + + updateRotors(); + + vehicle_api_->getStatusMessages(vehicle_api_messages_); + + if (getRemoteControlID() >= 0) + vehicle_api_->setRCData(getRCData()); + rotor_states_.timestamp = clock()->nowNanos(); + vehicle_api_->setRotorStates(rotor_states_); + vehicle_api_->setCollisionInfo(collision_info); +} + +void RovPawnSimApi::updateRendering(float dt) +{ + //if we did reset then don't worry about synchronizing states for this tick + if (reset_pending_) { + // Continue to wait for reset + if (!did_reset_) { + return; + } + else { + reset_pending_ = false; + did_reset_ = false; + return; + } + } + + if (!VectorMath::hasNan(last_phys_pose_)) { + if (pending_pose_status_ == PendingPoseStatus::RenderPending) { + PawnSimApi::setPose(last_phys_pose_, pending_pose_collisions_); + pending_pose_status_ = PendingPoseStatus::NonePending; + } + else + PawnSimApi::setPose(last_phys_pose_, false); + } + + //UAirBlueprintLib::LogMessage(TEXT("Collision (raw) Count:"), FString::FromInt(collision_response.collision_count_raw), LogDebugLevel::Unimportant); + UAirBlueprintLib::LogMessage(TEXT("Collision Count:"), + FString::FromInt(collision_response.collision_count_non_resting), + LogDebugLevel::Informational); + + for (auto i = 0; i < vehicle_api_messages_.size(); ++i) { + UAirBlueprintLib::LogMessage(FString(vehicle_api_messages_[i].c_str()), TEXT(""), LogDebugLevel::Success, 30); + } + + try { + vehicle_api_->sendTelemetry(dt); + } + catch (std::exception& e) { + UAirBlueprintLib::LogMessage(FString(e.what()), TEXT(""), LogDebugLevel::Failure, 30); + } + + pawn_events_->getActuatorSignal().emit(rotor_infos_); +} + +// update rotor_states_ and rotor_infos_ from aero_physics_body_ +void RovPawnSimApi::updateRotors() +{ + for (uint i = 0; i < rotor_count_; ++i) { + const auto& output = rov_physics_body_->getRotorOutput(i); + // update private rotor variable + rotor_states_.rotors[i].update( + output.rotor_output.thrust, + output.rotor_output.torque_scaler, + output.rotor_output.speed, + output.angle); + // create pointer to rotor_infos_[i] element and update data in-place + RotorTiltableInfo* info = &rotor_infos_[i]; + info->rotor_speed = output.rotor_output.speed; + info->rotor_direction = static_cast(output.rotor_output.turning_direction); + info->rotor_thrust = output.rotor_output.thrust; + info->rotor_control_filtered = output.rotor_output.control_signal_filtered; + info->rotor_angle_from_vertical = output.angle_from_vertical; + info->is_fixed = output.is_fixed; + } +} + +void RovPawnSimApi::setPose(const Pose& pose, bool ignore_collision) +{ + rov_physics_body_->lock(); + rov_physics_body_->setPose(pose); + rov_physics_body_->setGrounded(false); + rov_physics_body_->unlock(); + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +void RovPawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision) +{ + rov_physics_body_->lock(); + rov_physics_body_->updateKinematics(state); + rov_physics_body_->setGrounded(false); + rov_physics_body_->unlock(); + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +void RovPawnSimApi::setPoseCustom(const Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props) +{ + bool correct_num_angles = tilt_angles.size() == rotor_count_; + + rov_physics_body_->lock(); + rov_physics_body_->setPose(pose); + rov_physics_body_->setGrounded(false); + if (correct_num_angles) { + rov_physics_body_->overwriteRotorTilts(tilt_angles, spin_props); + } + updateRotors(); + rov_physics_body_->unlock(); + + // logging done outside of physics lock + if (!correct_num_angles) { + UAirBlueprintLib::LogMessage(TEXT("setPoseCustom called with wrong number of rotor angles: should have "), + FString::FromInt(rotor_count_), + LogDebugLevel::Failure, + 30); + } + + pending_pose_collisions_ = ignore_collision; + pending_pose_status_ = PendingPoseStatus::RenderPending; +} + +//*** Start: UpdatableState implementation ***// +void RovPawnSimApi::resetImplementation() +{ + PawnSimApi::resetImplementation(); + + vehicle_api_->reset(); + rov_physics_body_->reset(); + vehicle_api_messages_.clear(); +} + +//this is high frequency physics tick, flier gets ticked at rendering frame rate +void RovPawnSimApi::update() +{ + //environment update for current position + PawnSimApi::update(); + + //update forces on vertices + rov_physics_body_->update(); + + //update to controller must be done after kinematics have been updated by physics engine +} + +void RovPawnSimApi::reportState(StateReporter& reporter) +{ + PawnSimApi::reportState(reporter); + + rov_physics_body_->reportState(reporter); +} + +RovPawnSimApi::UpdatableObject* RovPawnSimApi::getPhysicsBody() +{ + return rov_physics_body_->getPhysicsBody(); +} +//*** End: UpdatableState implementation ***// diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h new file mode 100644 index 0000000000..7b1e20a5d4 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h @@ -0,0 +1,107 @@ +#pragma once + +#include "CoreMinimal.h" + +#include "PawnSimApi.h" +#include "vehicles/rov/RovPhysicsBody.hpp" +#include "vehicles/rov/RovBodyParams.hpp" +#include "common/Common.hpp" +#include "common/CommonStructs.hpp" +#include "common/common_utils/UniqueValueMap.hpp" +#include "RovPawnEvents.h" +#include +#include + +class RovPawnSimApi : public PawnSimApi +{ +public: + typedef msr::airlib::real_T real_T; + typedef msr::airlib::Utils Utils; + typedef msr::airlib::RovPhysicsBody RovPhysicsBody; + typedef msr::airlib::StateReporter StateReporter; + typedef msr::airlib::UpdatableObject UpdatableObject; + typedef msr::airlib::Pose Pose; + + typedef RovPawnEvents::RotorTiltableInfo RotorTiltableInfo; + +public: + virtual void initialize() override; + + virtual ~RovPawnSimApi() = default; + + //VehicleSimApiBase interface + //implements game interface to update pawn + RovPawnSimApi(const Params& params); + virtual void updateRenderedState(float dt) override; + virtual void updateRendering(float dt) override; + + //PhysicsBody interface + //this just wrapped around RovPhysicsBody physics body + virtual void resetImplementation() override; + virtual void update() override; + virtual void reportState(StateReporter& reporter) override; + virtual UpdatableObject* getPhysicsBody() override; + + virtual void setPose(const Pose& pose, bool ignore_collision) override; + virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override; + virtual void pawnTick(float dt) override; + + virtual void setPoseCustom(const Pose& pose, const vector& tilt_angles, bool ignore_collision, bool spin_props) override; + virtual void updateRotors(); + + msr::airlib::RovApiBase* getVehicleApi() const + { + return vehicle_api_.get(); + } + + virtual msr::airlib::VehicleApiBase* getVehicleApiBase() const override + { + return vehicle_api_.get(); + } + + void onKeyboardPwm(const std::vector& pwms); + + // 外部位姿源。设置后 updateRenderedState() 优先使用它,不再读取 + // rov_physics_body_,避免与外部物理引擎重复积分位姿。 + // provider 返回 false 时回退原有逻辑;坐标为 UE 世界系, + // 由基类 toPoseFromUnreal() 统一转换为 NED。 + void setExternalPoseProvider(std::function provider) + { + external_pose_provider_ = std::move(provider); + } + void clearExternalPoseProvider() { external_pose_provider_ = nullptr; } + bool hasExternalPoseProvider() const { return static_cast(external_pose_provider_); } + +private: + std::function external_pose_provider_; + + std::unique_ptr vehicle_api_; + std::unique_ptr vehicle_params_; + + std::unique_ptr rov_physics_body_; + unsigned int rotor_count_; + std::vector rotor_infos_; + + //show info on collision response from physics engine + CollisionResponse collision_response; + + RovPawnEvents* pawn_events_; + + //when pose needs to set from non-physics thread, we set it as pending + bool pending_pose_collisions_; + enum class PendingPoseStatus + { + NonePending, + RenderPending + } pending_pose_status_; + Pose pending_phys_pose_; //force new pose through API + + //reset must happen while World is locked so its async task initiated from API thread + bool reset_pending_; + bool did_reset_; + std::packaged_task reset_task_; + + Pose last_phys_pose_; //for trace lines showing vehicle path + std::vector vehicle_api_messages_; + RotorTiltableStates rotor_states_; +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp new file mode 100644 index 0000000000..dd38ecf3b9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp @@ -0,0 +1,228 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovStatusHUD.h" + +#include "Engine/Canvas.h" +#include "Engine/Engine.h" +#include "EngineUtils.h" +#include "Kismet/GameplayStatics.h" +#include "GameFramework/Pawn.h" +#include "GameFramework/PlayerController.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" + +ARovStatusHUD::ARovStatusHUD() +{ + PrimaryActorTick.bCanEverTick = true; +} + +void ARovStatusHUD::BeginPlay() +{ + Super::BeginPlay(); + ResolveBridge(); +} + +void ARovStatusHUD::ResolveBridge() +{ + if (Bridge.IsValid()) { + return; + } + UWorld* World = GetWorld(); + if (!World) { + return; + } + for (TActorIterator It(World); It; ++It) { + APawn* Pawn = *It; + if (!Pawn) { + continue; + } + URovMuJoCoBridge* Found = Pawn->FindComponentByClass(); + if (Found) { + Bridge = Found; + RovActor = Pawn; + return; + } + } +} + +void ARovStatusHUD::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + ResolveBridge(); + + if (APlayerController* PC = GetOwningPlayerController()) { + if (PC->WasInputKeyJustPressed(EKeys::R)) { + bShowStatusPanel = !bShowStatusPanel; + } + if (PC->WasInputKeyJustPressed(EKeys::V)) { + bShowVectors = !bShowVectors; + } + } +} + +void ARovStatusHUD::DrawHUD() +{ + Super::DrawHUD(); + if (!Canvas) { + return; + } + if (bShowStatusPanel) { + DrawStatusPanel(); + } + if (bShowVectors) { + DrawVectors(); + } +} + +void ARovStatusHUD::DrawStatusPanel() +{ + FRovStatusSnapshot S; + if (Bridge.IsValid()) { + Bridge->GetStatusSnapshot(S); + } + if (!S.bValid) { + DrawText(TEXT("ROV 状态:等待 MuJoCo 桥接…"), FLinearColor(1.0f, 0.8f, 0.2f), PanelOrigin.X, PanelOrigin.Y, nullptr, 1.1f); + return; + } + + const float X = bAnchorRight ? (Canvas->SizeX - 316.0f) : PanelOrigin.X; + float Y = PanelOrigin.Y; + const float LH = 20.0f; + const FLinearColor Title(0.4f, 0.9f, 1.0f); + const FLinearColor Body(0.92f, 0.95f, 1.0f); + const FLinearColor Warn(1.0f, 0.55f, 0.25f); + + DrawRect(FLinearColor(0.02f, 0.06f, 0.10f, 0.55f), X - 8.0f, Y - 8.0f, 300.0f, 8.0f * LH + 16.0f); + + DrawText(TEXT("ROV 实时状态"), Title, X, Y, nullptr, 1.2f); + Y += LH * 1.3f; + DrawText(FString::Printf(TEXT("姿态 横滚 %+6.1f° 俯仰 %+6.1f°"), S.RollDeg, S.PitchDeg), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT(" 航向 %+6.1f°"), S.YawDeg), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("深度 %.2f m"), S.DepthM), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("推力 %.1f N"), S.ThrustN), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("接触力 %.1f N"), S.ContactForceN), + S.ContactForceN > 1.0f ? Warn : Body, + X, + Y, + nullptr, + 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("洋流 %.3f m/s"), S.FlowVelocityMps.Size()), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("速度 %.3f m/s"), S.VelocityMps.Size()), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(TEXT("R 面板 V 向量"), FLinearColor(0.6f, 0.65f, 0.7f), X, Y, nullptr, 0.9f); +} + +void ARovStatusHUD::DrawVectors() +{ + if (!RovActor.IsValid() || !Bridge.IsValid() || !Canvas) { + return; + } + FRovStatusSnapshot S; + Bridge->GetStatusSnapshot(S); + if (!S.bValid) { + return; + } + + const FVector Origin = RovActor->GetActorLocation(); + + // UE 空间速度:用机体位置差分求方向,保证与屏幕上的运动方向一致; + // 幅值仍取 MuJoCo 的 qvel(更准)。两套坐标系不再混用。 + const float NowTime = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0f; + FVector VelUE = FVector::ZeroVector; + if (bHasLastRov) { + const float Dt = FMath::Max(NowTime - LastRovTime, 1e-3f); + VelUE = (Origin - LastRovLocation) / Dt; + } + LastRovLocation = Origin; + LastRovTime = NowTime; + bHasLastRov = true; + SmoothedVelocityUE = FMath::Lerp(SmoothedVelocityUE, VelUE, 0.35f); + + // 起终点都投影到屏幕,箭头方向与场景中的真实方向一致 + auto DrawWorldArrow = [this](const FVector& Start, const FVector& Dir, float LengthCm, const FLinearColor& Color, float Thickness) -> bool { + if (Dir.IsNearlyZero() || LengthCm <= 1.0f) { + return false; + } + const FVector End = Start + Dir.GetSafeNormal() * LengthCm; + const FVector P0 = Project(Start); + const FVector P1 = Project(End); + if (P0.Z <= 0.0f || P1.Z <= 0.0f) { + return false; + } + DrawLine(P0.X, P0.Y, P1.X, P1.Y, Color, Thickness); + + const FVector2D D(P1.X - P0.X, P1.Y - P0.Y); + const float L = D.Size(); + if (L < 6.0f) { + return true; + } + const FVector2D U = D / L; + const FVector2D N(-U.Y, U.X); + const float Head = FMath::Min(14.0f, L * 0.3f); + const FVector2D Base(P1.X - U.X * Head, P1.Y - U.Y * Head); + DrawLine(P1.X, P1.Y, Base.X + N.X * Head * 0.6f, Base.Y + N.Y * Head * 0.6f, Color, Thickness); + DrawLine(P1.X, P1.Y, Base.X - N.X * Head * 0.6f, Base.Y - N.Y * Head * 0.6f, Color, Thickness); + return true; + }; + + // 接触力:从接触点出发,沿实测法向力方向 + if (S.ContactForceN > 0.5f && !S.ContactForceDir.IsNearlyZero()) { + const float Len = FMath::Clamp(S.ContactForceN * ForceArrowScale, 30.0f, MaxArrowLength); + // 接触点存的是「相对机体、UE 轴、厘米」的偏移 + const FVector ContactUE = Origin + S.ContactPoint; + if (DrawWorldArrow(ContactUE, S.ContactForceDir, Len, FLinearColor(1.0f, 0.55f, 0.1f), 3.0f)) { + const FVector Tip = Project(ContactUE + S.ContactForceDir.GetSafeNormal() * Len); + if (Tip.Z > 0.0f) { + DrawText(FString::Printf(TEXT("F %.0f N"), S.ContactForceN), + FLinearColor(1.0f, 0.65f, 0.2f), + Tip.X + 8.0f, + Tip.Y - 10.0f, + nullptr, + 1.0f); + } + } + } + + // 机体速度:方向取 UE 位置差分,幅值取 MuJoCo 速度 + const float Speed = static_cast(S.VelocityMps.Size()); + if (Speed > 0.01f) { + const float Len = FMath::Clamp(Speed * VelocityArrowScale, 40.0f, MaxArrowLength); + FVector VelDir = SmoothedVelocityUE; + if (VelDir.Size() < 5.0f) { + // 低速时差分噪声大,退回 MuJoCo 速度(Y 镜像到 UE 轴) + VelDir = FVector(S.VelocityMps.X, -S.VelocityMps.Y, S.VelocityMps.Z); + } + if (DrawWorldArrow(Origin, VelDir, Len, FLinearColor(0.3f, 1.0f, 0.45f), 2.5f)) { + const FVector Tip = Project(Origin + VelDir.GetSafeNormal() * Len); + if (Tip.Z > 0.0f) { + DrawText(FString::Printf(TEXT("v %.2f m/s"), Speed), + FLinearColor(0.35f, 1.0f, 0.5f), + Tip.X + 8.0f, + Tip.Y + 2.0f, + nullptr, + 1.0f); + } + } + } + + // 洋流速度场:机体周围 3×3 采样,方向与幅值均取自实测洋流 + if (bShowFlowField) { + const FVector Flow = S.FlowVelocityMps; + const float FlowSpeed = static_cast(Flow.Size()); + if (FlowSpeed > 1e-3f) { + const FLinearColor FlowColor(0.35f, 0.7f, 1.0f, 0.85f); + const float Len = FMath::Clamp(FlowSpeed * 250.0f, 25.0f, 150.0f); + for (int32 iy = -1; iy <= 1; ++iy) { + for (int32 iz = -1; iz <= 1; ++iz) { + const FVector Sample = Origin + FVector(0.0f, iy * 150.0f, iz * 100.0f); + DrawWorldArrow(Sample, Flow, Len, FlowColor, 1.5f); + } + } + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h new file mode 100644 index 0000000000..5c6f9da795 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h @@ -0,0 +1,83 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 实时状态面板与力/速度向量可视化。 + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/HUD.h" +#include "RovStatusHUD.generated.h" + +class URovMuJoCoBridge; + +/** + * 屏幕左上角的状态面板 + 世界中的向量箭头。 + * + * 面板内容:姿态角、深度、推力、接触力、洋流速度。 + * 向量:夹爪接触力箭头、机体速度箭头、洋流速度场。 + * 可用 R 键切换面板、V 键切换向量显示。 + */ +UCLASS() +class AIRSIM_API ARovStatusHUD : public AHUD +{ + GENERATED_BODY() + +public: + ARovStatusHUD(); + + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void DrawHUD() override; + + /** 是否显示状态面板。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowStatusPanel = true; + + /** 是否显示力/速度向量。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowVectors = true; + + /** 是否显示洋流速度场(机体周围 3×3 采样箭头)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowFlowField = true; + + /** 面板左上角位置(屏幕像素)。bAnchorRight 为 true 时忽略 X,贴右边缘。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + FVector2D PanelOrigin = FVector2D(24.0f, 24.0f); + + /** 面板贴右上角,避开 AirSim 左上角的消息文字。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bAnchorRight = true; + + /** 力箭头比例(cm / N)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float ForceArrowScale = 0.5f; + + /** 速度箭头比例(cm / (m/s))。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float VelocityArrowScale = 250.0f; + + /** 箭头最长长度(cm),避免超出视野。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float MaxArrowLength = 300.0f; + +private: + void DrawStatusPanel(); + void DrawVectors(); + + UPROPERTY() + TWeakObjectPtr Bridge; + + UPROPERTY() + TWeakObjectPtr RovActor; + + /** 上一帧的机体位置与时间,用于求 UE 空间速度方向。 */ + FVector LastRovLocation = FVector::ZeroVector; + float LastRovTime = 0.0f; + bool bHasLastRov = false; + + /** 平滑后的 UE 空间速度(cm/s)。 */ + FVector SmoothedVelocityUE = FVector::ZeroVector; + + void ResolveBridge(); +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h new file mode 100644 index 0000000000..a6c1a3a945 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -0,0 +1,221 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 水下物理模型: +// - FRovOceanCurrent 三层级联洋流 +// - FRovPIDDepthController 定深 PID +// +// 头文件内联实现,不新增 .cpp。 + +#pragma once + +#include "CoreMinimal.h" +#include "Math/RandomStream.h" + +// ============================================================================= +// 洋流:三层级联 +// V_current(x,y,z,t) = V_GaussMarkov(t) + V_stratified(z) + V_turbulent(x,y,z) +// 坐标约定:MuJoCo 世界系,Z 向上,单位 m/s。 +// ============================================================================= +struct FRovOceanCurrent +{ + /** 分层剖面的一层 */ + struct FLayer + { + float DepthM = 0.0f; // 深度(负值表示水下) + float Speed = 0.0f; // 流速幅值 m/s + float HorizAngle = 0.0f; // 水平角 rad + float VertAngle = 0.0f; // 垂直角 rad(正 = 向下) + }; + + // ---------------------------- 配置 ---------------------------- + + bool bEnabled = true; + + // Gauss-Markov + float MeanSpeed = 0.5f; + float MeanHorizAngle = 0.0f; + float MeanVertAngle = 0.0f; + float Mu = 0.0f; + float NoiseAmp = 0.1f; + + // 分层 + TArray Layers; + + // 湍流 + float TurbIntensity = 0.15f; + float TurbIntegralScale = 1.0f; + + /** + * 洋流速度幅值上限(m/s)。Gauss-Markov 无均值回归时速度会无界增长, + * 这里加物理上限。 + */ + float MaxSpeed = 2.0f; + + int32 Seed = 42; + + // ---------------------------- 状态 ---------------------------- + + float CurSpeed = 0.0f; + float CurHoriz = 0.0f; + float CurVert = 0.0f; + + /** 最近一次 GetVelocity 的结果(MuJoCo 世界系,m/s),供状态显示读取。 */ + FVector LastVelocity = FVector::ZeroVector; + + FVector LastPos = FVector::ZeroVector; + bool bHasLastPos = false; + + FRandomStream Rng; + + void Initialize() + { + Rng.Initialize(Seed); + if (Layers.Num() == 0) { + // 与 config.json 的 stratified_layers 一致 + Layers.Add({ 0.0f, 0.0f, 0.0f, 0.0f }); + Layers.Add({ -10.0f, 0.5f, 0.3f, 0.0f }); + Layers.Add({ -50.0f, 1.0f, 0.5f, 0.1f }); + } + // 按深度升序(最深在前) + Layers.Sort([](const FLayer& A, const FLayer& B) { return A.DepthM < B.DepthM; }); + Reset(); + } + + void Reset() + { + CurSpeed = MeanSpeed; + CurHoriz = MeanHorizAngle; + CurVert = MeanVertAngle; + bHasLastPos = false; + LastPos = FVector::ZeroVector; + } + + /** Gauss-Markov 层:速度幅值 + 两个角度做均值回归随机游走 */ + FVector UpdateGaussMarkov(float Dt) + { + const float SqrtDt = FMath::Sqrt(FMath::Max(Dt, 1e-6f)); + + CurSpeed += Mu * (MeanSpeed - CurSpeed) * Dt + NoiseAmp * SqrtDt * Rng.GetFraction() * 2.0f - NoiseAmp * SqrtDt; + CurSpeed = FMath::Clamp(CurSpeed, 0.0f, MaxSpeed); + + CurHoriz += (Mu * (MeanHorizAngle - CurHoriz) + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + CurVert += (Mu * (MeanVertAngle - CurVert) + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + + const float CosV = FMath::Cos(CurVert); + // NED(Z 向下) → MuJoCo(Z 向上):vz 取反 + return FVector( + CurSpeed * CosV * FMath::Cos(CurHoriz), + CurSpeed * CosV * FMath::Sin(CurHoriz), + -CurSpeed * FMath::Sin(CurVert)); + } + + /** 分层层:按深度线性插值 */ + FVector StratifiedAt(float DepthM) const + { + if (Layers.Num() == 0) return FVector::ZeroVector; + + auto LayerVec = [](const FLayer& L) { + const float CosV = FMath::Cos(L.VertAngle); + return FVector( + L.Speed * CosV * FMath::Cos(L.HorizAngle), + L.Speed * CosV * FMath::Sin(L.HorizAngle), + -L.Speed * FMath::Sin(L.VertAngle)); + }; + + if (DepthM <= Layers[0].DepthM) { + return LayerVec(Layers[0]); + } + for (int32 i = 0; i < Layers.Num() - 1; ++i) { + if (DepthM >= Layers[i].DepthM && DepthM <= Layers[i + 1].DepthM) { + const float Span = Layers[i + 1].DepthM - Layers[i].DepthM; + const float T = (Span > KINDA_SMALL_NUMBER) + ? (DepthM - Layers[i].DepthM) / Span + : 0.0f; + return FMath::Lerp(LayerVec(Layers[i]), LayerVec(Layers[i + 1]), T); + } + } + return LayerVec(Layers.Last()); + } + + /** 湍流层:空间相关随机扰动 */ + FVector TurbulentAt(const FVector& Pos) + { + float NoiseStrength = TurbIntensity; + if (bHasLastPos) { + const float Dist = FVector::Dist(Pos, LastPos); + const float Correlation = FMath::Exp(-Dist / FMath::Max(TurbIntegralScale, 1e-6f)); + NoiseStrength = TurbIntensity * FMath::Sqrt(FMath::Max(0.0f, 1.0f - Correlation * Correlation)); + } + LastPos = Pos; + bHasLastPos = true; + + const FVector Noise( + Rng.GetFraction() * 2.0f - 1.0f, + Rng.GetFraction() * 2.0f - 1.0f, + Rng.GetFraction() * 2.0f - 1.0f); + return Noise * NoiseStrength * 0.5f; + } + + /** 总洋流速度(MuJoCo 世界系,m/s) */ + FVector GetVelocity(const FVector& Pos, float Dt) + { + if (!bEnabled) return FVector::ZeroVector; + + const FVector VGM = UpdateGaussMarkov(Dt); + const FVector VStrat = StratifiedAt(-Pos.Z); // z 向上 → 深度取反 + const FVector VTurb = TurbulentAt(Pos); + LastVelocity = VGM + VStrat + VTurb; + return LastVelocity; + } +}; + +// ============================================================================= +// PID 定深控制器 +// F = Kp·e + Ki·∫e·dt + Kd·ė + feedforward·V_current_z +// 坐标约定:Z 向上。正推力 = 向上。 +// ============================================================================= +struct FRovPIDDepthController +{ + // 增益基于 depth_controller.py 的 Kp=2000 / Ki=50 / Kd=1000, + // 但那组值是针对 150kg 的模型调的;本工程整机 612.9kg, + // 积分项积累太慢会留下约 0.5m 的稳态误差,所以 Ki 放大到 500。 + float Kp = 2000.0f; + float Ki = 500.0f; + float Kd = 1000.0f; + float MaxThrust = 5000.0f; + float FeedforwardGain = 500.0f; + + float IntegralError = 0.0f; + float PrevError = 0.0f; + + void Reset() + { + IntegralError = 0.0f; + PrevError = 0.0f; + } + + /** + * @param TargetZ 目标高度(MuJoCo 世界系,m) + * @param CurrentZ 当前高度 + * @param Dt 时间步长 + * @param CurrentVz 当前位置的洋流 Z 分量(正 = 向上) + * @return 需要的 Z 轴推力(N),已限幅 + */ + float Compute(float TargetZ, float CurrentZ, float Dt, float CurrentVz) + { + const float Error = TargetZ - CurrentZ; + + IntegralError += Error * Dt; + IntegralError = FMath::Clamp(IntegralError, -100.0f, 100.0f); // anti-windup + + const float Derivative = (Dt > KINDA_SMALL_NUMBER) ? (Error - PrevError) / Dt : 0.0f; + + float Thrust = Kp * Error + Ki * IntegralError + Kd * Derivative; + + // 洋流前馈:洋流向上(+vz)时提前减少推力 + Thrust += FeedforwardGain * CurrentVz; + + PrevError = Error; + return FMath::Clamp(Thrust, -MaxThrust, MaxThrust); + } +}; diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp new file mode 100644 index 0000000000..7fdcdb3859 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp @@ -0,0 +1,194 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#include "SimModeWorldRov.h" +#include "UObject/ConstructorHelpers.h" +#include "Logging/MessageLog.h" +#include "Engine/World.h" +#include "GameFramework/PlayerController.h" +#include "Vehicles/Rov/RovObstacleField.h" +#include "Vehicles/Rov/RovStatusHUD.h" + +#include "AirBlueprintLib.h" +#include "vehicles/rov/api/RovApiBase.hpp" +#include "RovPawnSimApi.h" +#include "physics/PhysicsBody.hpp" +#include "common/ClockFactory.hpp" +#include +#include "vehicles/rov/api/RovRpcLibServer.hpp" +#include "common/SteppableClock.hpp" + +void ASimModeWorldRov::BeginPlay() +{ + Super::BeginPlay(); + + //let base class setup physics world + initializeForPlay(); + + // 障碍物场景(7.3.2):管道 / 框架 / 礁石,可被关卡里手动摆放的实例替代 + if (bSpawnObstacleField && GetWorld()) { + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + ARovObstacleField* Field = GetWorld()->SpawnActor(ARovObstacleField::StaticClass(), FTransform::Identity, SpawnParams); + if (Field) { + // 以 ROV 出生点为锚点,保证视觉代理与 MJCF 里的碰撞几何体位置一致 + TArray Pawns; + getExistingVehiclePawns(Pawns); + if (Pawns.Num() > 0 && Pawns[0]) { + Field->AnchorActor = Pawns[0]; + Field->Rebuild(); + } + } + UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物场已生成:%s"), Field ? TEXT("成功") : TEXT("失败")); + } + + // 实时状态面板 + 力/速度向量(3.4 / 7.3.1) + if (bSpawnStatusHUD && GetWorld()) { + if (APlayerController* PC = GetWorld()->GetFirstPlayerController()) { + PC->ClientSetHUD(ARovStatusHUD::StaticClass()); + UE_LOG(LogTemp, Log, TEXT("[RovScene] 实时状态面板已启用")); + } + } +} + +void ASimModeWorldRov::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + //stop physics thread before we dismantle + stopAsyncUpdator(); + + Super::EndPlay(EndPlayReason); +} + +void ASimModeWorldRov::setupClockSpeed() +{ + typedef msr::airlib::ClockFactory ClockFactory; + + float clock_speed = getSettings().clock_speed; + + //setup clock in ClockFactory + std::string clock_type = getSettings().clock_type; + + if (clock_type == "ScalableClock") { + //scalable clock returns interval same as wall clock but multiplied by a scale factor + ClockFactory::get(std::make_shared(clock_speed == 1 ? 1 : 1 / clock_speed)); + } + else if (clock_type == "SteppableClock") { + //steppable clock returns interval that is a constant number irrespective of wall clock + //we can either multiply this fixed interval by scale factor to speed up/down the clock + //but that would cause vehicles like quadrotors to become unstable + //so alternative we use here is instead to scale control loop frequency. The downside is that + //depending on compute power available, we will max out control loop frequency and therefore can no longer + //get increase in clock speed + + //Approach 1: scale clock period, no longer used now due to quadrotor instability + //ClockFactory::get(std::make_shared( + //static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + + //Approach 2: scale control loop frequency if clock is speeded up + if (clock_speed >= 1) { + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9))); //no clock_speed multiplier + + setPhysicsLoopPeriod(getPhysicsLoopPeriod() / static_cast(clock_speed)); + } + else { + //for slowing down, this don't generate instability + ClockFactory::get(std::make_shared( + static_cast(getPhysicsLoopPeriod() * 1E-9 * clock_speed))); + } + } + else + throw std::invalid_argument(common_utils::Utils::stringf( + "clock_type %s is not recognized", clock_type.c_str())); +} + +//-------------------------------- overrides -----------------------------------------------// + +APawn* ASimModeWorldRov::createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) +{ + try { + return ASimModeBase::createVehiclePawn(vehicle_setting); + } + catch (const std::exception& e) { + UAirBlueprintLib::LogMessageString("createVehiclePawn fallback to native ARovPawn: ", e.what(), LogDebugLevel::Informational); + + const FTransform uu_origin = getGlobalNedTransform().getGlobalTransform(); + FVector spawn_position = uu_origin.GetLocation(); + if (!msr::airlib::VectorMath::hasNan(vehicle_setting.position)) + spawn_position = getGlobalNedTransform().fromGlobalNed(vehicle_setting.position); + + FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); + + FActorSpawnParameters pawn_spawn_params; + pawn_spawn_params.Name = FName(vehicle_setting.vehicle_name.c_str()); + pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + + APawn* spawned_pawn = this->GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params); + if (spawned_pawn) { + spawned_actors_.Add(spawned_pawn); + return spawned_pawn; + } + throw; + } +} + +std::unique_ptr ASimModeWorldRov::createApiServer() const +{ +#ifdef AIRLIB_NO_RPC + return ASimModeBase::createApiServer(); +#else + UE_LOG(LogTemp, Log, TEXT("SimModeWorldRov: Api server started on port %d"), getSettings().api_port); + return std::unique_ptr(new msr::airlib::RovRpcLibServer( + getApiProvider(), getSettings().api_server_address, getSettings().api_port)); +#endif +} + +void ASimModeWorldRov::getExistingVehiclePawns(TArray& pawns) const +{ + UAirBlueprintLib::FindAllActor(this, pawns); +} + +bool ASimModeWorldRov::isVehicleTypeSupported(const std::string& vehicle_type) const +{ + return ((vehicle_type == AirSimSettings::kVehicleTypeRovSimple) || + (vehicle_type == AirSimSettings::kVehicleTypePX4Rov)); +} + +std::string ASimModeWorldRov::getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const +{ + //decide which derived BP to use + std::string pawn_path = vehicle_setting.pawn_path; + if (pawn_path == "") + pawn_path = "DefaultRov"; + + return pawn_path; +} + +PawnEvents* ASimModeWorldRov::getVehiclePawnEvents(APawn* pawn) const +{ + return static_cast(pawn)->getPawnEvents(); +} +const common_utils::UniqueValueMap ASimModeWorldRov::getVehiclePawnCameras( + APawn* pawn) const +{ + return (static_cast(pawn))->getCameras(); +} +void ASimModeWorldRov::initializeVehiclePawn(APawn* pawn) +{ + static_cast(pawn)->initializeForBeginPlay(); +} +std::unique_ptr ASimModeWorldRov::createVehicleSimApi( + const PawnSimApi::Params& pawn_sim_api_params) const +{ + auto vehicle_sim_api = std::unique_ptr(new RovPawnSimApi(pawn_sim_api_params)); + vehicle_sim_api->initialize(); + //For multirotors the vehicle_sim_api are in PhysicsWOrld container and then get reseted when world gets reseted + //vehicle_sim_api->reset(); + return vehicle_sim_api; +} +msr::airlib::VehicleApiBase* ASimModeWorldRov::getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, + const PawnSimApi* sim_api) const +{ + const auto Rov_sim_api = static_cast(sim_api); + return Rov_sim_api->getVehicleApi(); +} diff --git a/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h new file mode 100644 index 0000000000..f2138560dd --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h @@ -0,0 +1,45 @@ +#pragma once + +#include "CoreMinimal.h" + +#include "RovPawn.h" +#include "common/Common.hpp" +#include "SimMode/SimModeWorldBase.h" +#include "api/VehicleSimApiBase.hpp" +#include "SimModeWorldRov.generated.h" + +UCLASS() +class AIRSIM_API ASimModeWorldRov : public ASimModeWorldBase +{ + GENERATED_BODY() + +public: + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + + /** 是否自动生成障碍物场景(管道 / 框架 / 礁石)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV") + bool bSpawnObstacleField = true; + + /** 是否启用 ROV 实时状态面板与力/速度向量显示。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV") + bool bSpawnStatusHUD = true; + +protected: //overrides + virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) override; + virtual void setupClockSpeed() override; + virtual std::unique_ptr createApiServer() const override; + virtual void getExistingVehiclePawns(TArray& pawns) const override; + virtual bool isVehicleTypeSupported(const std::string& vehicle_type) const override; + virtual std::string getVehiclePawnPathName(const AirSimSettings::VehicleSetting& vehicle_setting) const override; + virtual PawnEvents* getVehiclePawnEvents(APawn* pawn) const override; + virtual const common_utils::UniqueValueMap getVehiclePawnCameras(APawn* pawn) const override; + virtual void initializeVehiclePawn(APawn* pawn) override; + virtual std::unique_ptr createVehicleSimApi( + const PawnSimApi::Params& pawn_sim_api_params) const override; + virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, + const PawnSimApi* sim_api) const override; + +private: + typedef ARovPawn TVehiclePawn; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.cpp index f18e734313..b70dfa65e1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.cpp @@ -21,12 +21,36 @@ void FCarlaModule::StartupModule() void FCarlaModule::AddShaderSearchPaths() { - FString PluginBaseDir = IPluginManager::Get().FindPlugin(TEXT("Carla"))->GetBaseDir(); - UE_LOG(LogCarla, Log, TEXT("PluginBaseDir: %s"), *PluginBaseDir); - check(FPaths::DirectoryExists(PluginBaseDir)); + // OpenHUTB 修改(2026-11-06):打包版启动即崩的根因。 + // + // 原代码用 check() 断言插件目录和 Shaders 子目录必须存在。 + // 但打包版会把插件资源 cook 进 pak,磁盘上既没有 Plugins/Carla 目录, + // 更没有 Shaders 子目录 —— check() 直接断言失败,进程崩在启动阶段, + // 界面上只显示一个 "Fatal error" 对话框。 + // + // 改为:找不到就记录警告并跳过。打包版本来也不需要这个 shader 源映射 + // (shader 已经编译进 pak),只有编辑器里热重载 shader 时才需要。 + IPluginManager& PluginManager = IPluginManager::Get(); + TSharedPtr Plugin = PluginManager.FindPlugin(TEXT("Carla")); + if (!Plugin.IsValid()) + { + UE_LOG(LogCarla, Warning, + TEXT("Carla 插件未找到,跳过 shader 源路径注册")); + return; + } + + const FString PluginBaseDir = Plugin->GetBaseDir(); FString ShadersDirectoryPath = FPaths::Combine(PluginBaseDir, TEXT("Shaders")); UE_LOG(LogCarla, Log, TEXT("ShadersDirectoryPath: %s"), *ShadersDirectoryPath); - check(FPaths::DirectoryExists(ShadersDirectoryPath)); + + if (!FPaths::DirectoryExists(ShadersDirectoryPath)) + { + UE_LOG(LogCarla, Warning, + TEXT("Shaders 目录不存在(打包版属正常,资源已在 pak 内):%s —— 跳过 shader 源映射"), + *ShadersDirectoryPath); + return; + } + ShadersDirectoryPath = FPaths::ConvertRelativePathToFull(ShadersDirectoryPath); AddShaderSourceDirectoryMapping("/Plugin/Carla", ShadersDirectoryPath); } @@ -147,9 +171,15 @@ IMPLEMENT_MODULE(FCarlaModule, Carla) namespace carla { void throw_exception(const std::exception &e) { - UE_LOG(LogCarla, Fatal, TEXT("Exception thrown: %s"), UTF8_TO_TCHAR(e.what())); - // It should never reach this part. - std::terminate(); + // OpenHUTB 修改(2026-11-06): + // 原来是 UE_LOG(Fatal) + std::terminate(),调用方完全没有捕获机会。 + // 实测后果:本机已有一个 Carla 实例(编辑器或另一个打包版)占着 + // 2000/2001/2002 时,打包版启动 40~70 秒后直接崩掉,界面上只显示 + // 一个 "Fatal error" 对话框,排查成本极高。 + // + // 改为记录错误后正常抛出,让上层(FCarlaServer::Start)可以捕获并降级。 + UE_LOG(LogCarla, Error, TEXT("Exception thrown: %s"), UTF8_TO_TCHAR(e.what())); + throw e; } } // namespace carla diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index e64e00cfe1..1b731cd43c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -3803,7 +3803,27 @@ FCarlaServer::~FCarlaServer() { FDataMultiStream FCarlaServer::Start(uint16_t RPCPort, uint16_t StreamingPort, uint16_t SecondaryPort) { - Pimpl = MakeUnique(RPCPort, StreamingPort, SecondaryPort); + // OpenHUTB 修改(2026-11-06):端口被占用时降级,不再崩整个进程。 + // + // 实测:本机已有另一个 Carla 实例(编辑器或另一个打包版)占着 + // 2000/2001/2002 时,打包版会在启动 40~70 秒后直接 Fatal 退出。 + // 对纯演示/离线场景来说 Carla 的 Python API 并非必需, + // 所以这里记录错误并继续,让场景照常能看。 + try + { + Pimpl = MakeUnique(RPCPort, StreamingPort, SecondaryPort); + } + catch (const std::exception &e) + { + UE_LOG(LogCarla, Error, + TEXT("CarlaServer 启动失败(端口 rpc=%d / streaming=%d / secondary=%d 可能被占用):%s"), + RPCPort, StreamingPort, SecondaryPort, UTF8_TO_TCHAR(e.what())); + UE_LOG(LogCarla, Warning, + TEXT("已降级为「不启动 Carla 服务器」继续运行 —— 场景可正常查看,但外部 Python API 不可用。" + "如需 API,请先关闭本机其它 Carla 实例再启动。")); + return FDataMultiStream(); + } + StreamingPort = Pimpl->StreamingServer.GetLocalEndpoint().port(); SecondaryPort = Pimpl->SecondaryServer->GetLocalEndpoint().port(); @@ -3819,20 +3839,20 @@ FDataMultiStream FCarlaServer::Start(uint16_t RPCPort, uint16_t StreamingPort, u void FCarlaServer::NotifyBeginEpisode(UCarlaEpisode &Episode) { - check(Pimpl != nullptr); + if (Pimpl == nullptr) return; // OpenHUTB:端口绑定失败后的降级路径 UE_LOG(LogCarlaServer, Log, TEXT("New episode '%s' started"), *Episode.GetMapName()); Pimpl->Episode = &Episode; } void FCarlaServer::NotifyEndEpisode() { - check(Pimpl != nullptr); + if (Pimpl == nullptr) return; // OpenHUTB:降级路径 Pimpl->NotifyEndEpisode(); } void FCarlaServer::AsyncRun(uint32 NumberOfWorkerThreads) { - check(Pimpl != nullptr); + if (Pimpl == nullptr) return; // OpenHUTB:降级路径 /// @todo Define better the number of threads each server gets. int ThreadsPerServer = std::max(2u, NumberOfWorkerThreads / 3u); int32_t RPCThreads; @@ -3864,6 +3884,7 @@ void FCarlaServer::AsyncRun(uint32 NumberOfWorkerThreads) void FCarlaServer::RunSome(uint32 Milliseconds) { + if (Pimpl == nullptr) return; // OpenHUTB:降级路径 TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__); Pimpl->Server.SyncRunFor(carla::time_duration::milliseconds(Milliseconds)); } @@ -3891,6 +3912,7 @@ double FCarlaServer::GetTickDeltaSeconds() { void FCarlaServer::Tick() { + if (Pimpl == nullptr) return; // OpenHUTB:降级路径 (void)Pimpl->call_tick(Pimpl->SynchronizationClientId(), Pimpl->ServerSynchronizationParticipantId, carla::rpc::SynchronizationTickMode::TICK_ONLY_IF_SYNC_ENABLED); @@ -3912,7 +3934,7 @@ void FCarlaServer::Stop() FDataStream FCarlaServer::OpenStream() const { - check(Pimpl != nullptr); + if (Pimpl == nullptr) return FDataStream(); // OpenHUTB:降级路径 return Pimpl->StreamingServer.MakeStream(); } diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin b/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin index e5ca97b4af..2b8e91a8ec 100644 --- a/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin @@ -17,7 +17,7 @@ "Modules": [ { "Name": "Holodeck", - "Type": "Editor", + "Type": "Runtime", "LoadingPhase": "Default" } ] diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/LICENSE b/Unreal/CarlaUE4/Plugins/HoloOcean/LICENSE new file mode 100644 index 0000000000..19a7558efd --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 BYU PCCL + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/NOTICE.md b/Unreal/CarlaUE4/Plugins/HoloOcean/NOTICE.md new file mode 100644 index 0000000000..6060d0987c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/NOTICE.md @@ -0,0 +1,62 @@ +# 来源与许可说明 + +本插件(目录名 `HoloOcean`,模块名 `Holodeck`)基于开源项目 **HoloOcean / Holodeck**(BYU PCCL)修改而来, +以 **MIT License** 授权,全文见同目录 [`LICENSE`](LICENSE)。 + +## 原始版权 + +``` +Copyright (c) 2019 BYU PCCL +``` + +`Source/Holodeck/Holodeck.Build.cs` 首行保留原始声明: + +```cpp +// MIT License (c) 2019 BYU PCCL see LICENSE file +``` + +## 本副本的改动 + +OpenHUTB 在引入时对插件做了以下改动,均基于 MIT 许可允许的范围: + +- `HoloOcean.uplugin` 中的 `CreatedBy` 字段被改为 `OpenHUTB`(原为 BYU PCCL) +- `DocsURL` / `SupportURL` 指向 OpenHUTB 的仓库与文档站 +- 模块类型由 `Editor` 调整为 `Runtime`,以便在打包构建中使用(2026-11-04) +- `Source/Holodeck/Holodeck.cpp`:`IMPLEMENT_PRIMARY_GAME_MODULE` 改为 `IMPLEMENT_MODULE` + (修复打包时 LNK2005 重复符号,详见下节) + +### 关于 IMPLEMENT_MODULE 的修改 + +`Holodeck.cpp` 原使用: + +```cpp +IMPLEMENT_PRIMARY_GAME_MODULE( FDefaultGameModuleImpl, Holodeck, "Holodeck" ); +``` + +该宏专供工程的「主游戏模块」使用,会定义 `GInternalProjectName`、`GLiveCodingProject`、 +`GObjectArrayForDebugVisualizers`、`operator new/delete` 等一整套全局符号。 + +编辑器构建时模块编成独立 DLL,不冲突;但模块类型改为 `Runtime` 后, +游戏目标会把插件模块链接进单体可执行文件,与工程主模块 `CarlaUE4.cpp` 重复定义: + +``` +error LNK2005: "operator new" already defined in Module.Holodeck.cpp.obj +error LNK2005: "wchar_t * GInternalProjectName" already defined in Module.Holodeck.cpp.obj +fatal error LNK1169: one or more multiply defined symbols found +``` + +改为 `IMPLEMENT_MODULE( FDefaultGameModuleImpl, Holodeck );` 后打包正常。 + +**这是上游 HoloOcean 的缺陷**——插件本就不应使用 `IMPLEMENT_PRIMARY_GAME_MODULE`。 + +## MIT 合规要求 + +MIT 许可要求:**在软件的所有副本或实质性部分中,保留上述版权声明与本许可声明。** + +因此本目录必须始终保留 `LICENSE` 文件。打包分发(exe / 安装器)时, +需将本文件一并纳入安装包的许可声明目录。 + +## 上游项目 + +- HoloOcean 官方文档:https://holoocean.readthedocs.io/ +- 维护方:Brigham Young University, Perception, Control and Cognition Lab (BYU PCCL) diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp index 8f8d2ca51f..881b922112 100644 --- a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp @@ -32,6 +32,18 @@ FVector Octree::EnvCenter; UWorld* Octree::World; TDiscardableKeyValueCache Octree::materials; +// OpenHUTB 追加 +bool Octree::bExplicitBounds = false; + +void Octree::setEnvironmentBounds(const FVector& InMin, const FVector& InMax){ + // 传入即为 UE 厘米单位,无需再做 ClientToUE 换算 + EnvMin = InMin; + EnvMax = InMax; + bExplicitBounds = true; + UE_LOG(LogHolodeck, Log, TEXT("Octree:: setEnvironmentBounds min=%s max=%s"), + *EnvMin.ToString(), *EnvMax.ToString()); +} + float sign(float val){ bool s = signbit(val); if(s) return -1.0; @@ -42,15 +54,18 @@ void Octree::initOctree(UWorld* w){ World = w; // Load environment size - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinX="), EnvMin.X)) EnvMin.X = -10; - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinY="), EnvMin.Y)) EnvMin.Y = -10; - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinZ="), EnvMin.Z)) EnvMin.Z = -10; - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxX="), EnvMax.X)) EnvMax.X = 10; - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxY="), EnvMax.Y)) EnvMax.Y = 10; - if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxZ="), EnvMax.Z)) EnvMax.Z = 10; - // Clean environment size - EnvMin = ConvertLinearVector(EnvMin, ClientToUE); - EnvMax = ConvertLinearVector(EnvMax, ClientToUE); + // OpenHUTB 追加:若已通过 setEnvironmentBounds 显式指定,则跳过命令行解析与单位换算 + if (!bExplicitBounds) { + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinX="), EnvMin.X)) EnvMin.X = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinY="), EnvMin.Y)) EnvMin.Y = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinZ="), EnvMin.Z)) EnvMin.Z = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxX="), EnvMax.X)) EnvMax.X = 10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxY="), EnvMax.Y)) EnvMax.Y = 10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxZ="), EnvMax.Z)) EnvMax.Z = 10; + // Clean environment size + EnvMin = ConvertLinearVector(EnvMin, ClientToUE); + EnvMax = ConvertLinearVector(EnvMax, ClientToUE); + } FVector min = FVector((int)FGenericPlatformMath::Min(EnvMin.X, EnvMax.X), (int)FGenericPlatformMath::Min(EnvMin.Y, EnvMax.Y), (int)FGenericPlatformMath::Min(EnvMin.Z, EnvMax.Z)); FVector max = FVector((int)FGenericPlatformMath::Max(EnvMin.X, EnvMax.X), (int)FGenericPlatformMath::Max(EnvMin.Y, EnvMax.Y), (int)FGenericPlatformMath::Max(EnvMin.Z, EnvMax.Z)); EnvMin = min; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h index 1cd29d6c14..0d50b49db5 100644 --- a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h @@ -32,6 +32,9 @@ class Octree static UWorld* World; static TDiscardableKeyValueCache materials; + /** OpenHUTB 追加:为 true 时 initOctree 跳过命令行解析,直接使用 setEnvironmentBounds 设置的值 */ + static bool bExplicitBounds; + static FVector EnvCenter; static void loadJson(gason::JsonValue& json, TArray& parent, float size); @@ -55,6 +58,16 @@ class Octree static float OctreeMax; static float OctreeMin; + /** + * 直接指定环境包围盒(UE 厘米单位),跳过命令行参数。 + * 必须在 initOctree 之前调用。 + * + * OpenHUTB 追加:HoloOcean 原本只支持通过 -EnvMinX= / -EnvMaxX= 等命令行 + * 参数指定环境范围(单位米),与 Carla 集成时没有传参渠道。 + * 提供此接口后,可由 UnderwaterSensorSubsystem 按场景实际包围盒设置。 + */ + static void setEnvironmentBounds(const FVector& InMin, const FVector& InMax); + Octree(){}; Octree(FVector loc, float size, FString file="") : size(size), loc(loc), file(file) {}; ~Octree(){ diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp index 9f3d5d19cf..e92a4bc09d 100644 --- a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp @@ -2,6 +2,11 @@ #include "Holodeck.h" -IMPLEMENT_PRIMARY_GAME_MODULE( FDefaultGameModuleImpl, Holodeck, "Holodeck" ); +// OpenHUTB 修改(2026-11-04): +// 原为 IMPLEMENT_PRIMARY_GAME_MODULE。该宏专供工程的「主游戏模块」使用, +// 会定义 GInternalProjectName / GLiveCodingProject / operator new / delete 等全局符号。 +// 本插件作为 Runtime 模块被链接进单体可执行文件时,会与 CarlaUE4 主模块重复定义, +// 导致 LNK2005 / LNK1169。插件应使用 IMPLEMENT_MODULE。 +IMPLEMENT_MODULE( FDefaultGameModuleImpl, Holodeck ); DEFINE_LOG_CATEGORY(LogHolodeck); \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorHost.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorHost.cpp new file mode 100644 index 0000000000..4e14b0a1dd --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorHost.cpp @@ -0,0 +1,29 @@ +// MIT License — OpenHUTB 水下整合层 + +#include "Holodeck.h" +#include "UnderwaterSensorHost.h" +#include "UnderwaterSensorSubsystem.h" + +#include "Engine/World.h" + +AUnderwaterSensorHost::AUnderwaterSensorHost(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) +{ + // 不接管 pawn,也不需要 tick + PrimaryActorTick.bCanEverTick = false; + UE_LOG(LogHolodeck, Log, TEXT("UnderwaterSensorHost created")); +} + +void AUnderwaterSensorHost::BeginPlay() +{ + Super::BeginPlay(); + + // 场景已加载完毕,让子系统初始化八叉树(声呐射线依赖它) + if (UWorld* World = GetWorld()) + { + if (UUnderwaterSensorSubsystem* Subsystem = World->GetSubsystem()) + { + Subsystem->EnsureOctreeReady(); + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorSubsystem.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorSubsystem.cpp new file mode 100644 index 0000000000..872f764407 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/UnderwaterSensorSubsystem.cpp @@ -0,0 +1,282 @@ +// MIT License — OpenHUTB 水下整合层 + +#include "Holodeck.h" +#include "UnderwaterSensorSubsystem.h" + +#include "UnderwaterSensorHost.h" +#include "HolodeckServer.h" +#include "HolodeckSensor.h" +#include "LocationSensor.h" +#include "ImagingSonar.h" +#include "Octree.h" + +#include "Engine/World.h" +#include "Components/SceneComponent.h" +#include "TimerManager.h" + +void UUnderwaterSensorSubsystem::Initialize(FSubsystemCollectionBase& Collection) +{ + Super::Initialize(Collection); + + UWorld* World = GetWorld(); + if (World == nullptr) + { + UE_LOG(LogHolodeck, Warning, TEXT("UnderwaterSensorSubsystem: no world, skip init")); + return; + } + + // 只允许在游戏世界初始化,编辑器预览世界不启动 + if (World->WorldType != EWorldType::Game && World->WorldType != EWorldType::PIE) + { + UE_LOG(LogHolodeck, Log, TEXT("UnderwaterSensorSubsystem: world type %d, skip"), (int32)World->WorldType); + return; + } + + // 注意:这里只创建 Server 与 Host,不初始化八叉树。 + // 八叉树依赖场景中已加载的 Actor,必须推迟到 OnWorldBeginPlay。 + + // 1. 创建共享内存分配器 + // 注意:刻意不调用 Server->Start()——那会打开命名信号量并进入与 Python 客户端的 + // 锁步同步。我们的数据在进程内消费(经 ZMQ 转发给 ROS2),不需要跨进程共享。 + Server = NewObject(this); + + // 2. 创建传感器宿主 + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + SpawnParams.Name = TEXT("UnderwaterSensorHost"); + Host = World->SpawnActor( + AUnderwaterSensorHost::StaticClass(), FTransform::Identity, SpawnParams); + + if (Host != nullptr) + { + Host->SetServer(Server); + UE_LOG(LogHolodeck, Log, TEXT("UnderwaterSensorSubsystem: initialized, server=%p host=%p"), + Server, Host); + } + else + { + UE_LOG(LogHolodeck, Error, TEXT("UnderwaterSensorSubsystem: failed to spawn sensor host")); + } +} + +void UUnderwaterSensorSubsystem::EnsureOctreeReady() +{ + if (bOctreeReady) + { + return; + } + + UWorld* World = GetWorld(); + if (World == nullptr) + { + return; + } + + // 到这里场景中的 Actor 已经加载完毕,八叉树才能扫到真实几何。 + // 之前在 Initialize 阶段调用会得到 ±1000 的默认包围盒(空场景),声呐射线全部落空。 + // + // 另外:HoloOcean 原本只从命令行读环境范围(-EnvMinX= 等,单位米), + // 缺省会退化成 ±10 米,对 Town10HD 这种公里级地图完全不够。 + // + // 不能简单遍历 Actor 累加包围盒——天空球/大气等 Actor 的包围盒接近无限大 + // (实测得到 ±500 km),会让八叉树规模爆炸。 + // 因此改用可配置的局部作业区范围,默认以原点为中心 400m × 400m × 100m。 + const FVector Center = EnvironmentCenter; + const FVector Extent = EnvironmentExtent; + Octree::setEnvironmentBounds(Center - Extent, Center + Extent); + + Octree::initOctree(World); + bOctreeReady = true; + + UE_LOG(LogHolodeck, Log, TEXT("UnderwaterSensorSubsystem: octree initialized for world '%s'"), + *World->GetName()); + + // 命令行开关触发的通路自检 + if (FParse::Param(FCommandLine::Get(), TEXT("UnderwaterSensorTest"))) + { + RunSensorSelfTest(); + } +} + +void UUnderwaterSensorSubsystem::RunSensorSelfTest() +{ + UWorld* World = GetWorld(); + if (World == nullptr) + { + return; + } + + // 1. 承载传感器用的 Actor(带根组件,LocationSensor 需要 attach parent) + // 刻意放在一个已知偏移处,这样读回的值可以反推坐标转换是否正确: + // UE 厘米 (1000, 2000, 200) + // → UEToClient: ÷100 → (10, 20, 2) + // → USE_RHS: Y 取反 → (10, -20, 2) + // Z 取 +200cm(地面上方 2m),否则声呐埋在地下看不到建筑 + const FVector TestOffset(1000.f, 2000.f, 200.f); + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + AActor* TestActor = World->SpawnActor( + AActor::StaticClass(), EnvironmentCenter + TestOffset, FRotator::ZeroRotator, SpawnParams); + if (TestActor == nullptr) + { + UE_LOG(LogHolodeck, Error, TEXT("SelfTest: failed to spawn test actor")); + return; + } + + USceneComponent* Root = NewObject(TestActor, TEXT("TestRoot")); + TestActor->SetRootComponent(Root); + Root->RegisterComponent(); + + // 显式设置位置:AActor::StaticClass() 生成时没有根组件,SpawnActor 传入的位置可能不生效 + TestActor->SetActorLocation(EnvironmentCenter + TestOffset); + UE_LOG(LogHolodeck, Log, TEXT("SelfTest: actor spawned at %s"), *TestActor->GetActorLocation().ToString()); + + // 2. 挂 LocationSensor(3 个 float,无需额外参数,适合做通路冒烟测试) + ULocationSensor* LocSensor = NewObject(TestActor, TEXT("TestLocationSensor")); + LocSensor->SetupAttachment(Root); + LocSensor->RegisterComponent(); + + // 3. 接入子系统(顺序不能反:先 SetAgentAndController 再 InitializeSensor) + if (!AttachSensor(LocSensor, TEXT("selftest"))) + { + UE_LOG(LogHolodeck, Error, TEXT("SelfTest: AttachSensor failed")); + return; + } + + UE_LOG(LogHolodeck, Log, TEXT("SelfTest: LocationSensor attached, buffer=%p size=%d"), + LocSensor->GetDataBuffer(), LocSensor->GetDataBufferSize()); + + // 4. 等两秒让传感器 tick 若干帧,再读缓冲区 + FTimerHandle Handle; + TWeakObjectPtr WeakSensor(LocSensor); + World->GetTimerManager().SetTimer(Handle, [WeakSensor]() + { + ULocationSensor* Sensor = WeakSensor.Get(); + if (Sensor == nullptr) + { + UE_LOG(LogHolodeck, Warning, TEXT("SelfTest: sensor gone")); + return; + } + + const float* Data = static_cast(Sensor->GetDataBuffer()); + if (Data == nullptr) + { + UE_LOG(LogHolodeck, Error, TEXT("SelfTest: buffer is null")); + return; + } + + UE_LOG(LogHolodeck, Warning, + TEXT("SelfTest RESULT: Location = (%.2f, %.2f, %.2f) [expected (10.00, -20.00, 2.00) for UE offset (1000, 2000, 200)]"), + Data[0], Data[1], Data[2]); + }, 2.0f, false); + + // ------------------------------------------------------------------ + // 5. 再挂一个 ImagingSonar,验证真实声呐通路 + // 参数单位:RangeMin/RangeMax 为米(内部 ×100 转厘米),角度为度 + // ------------------------------------------------------------------ + UImagingSonar* Sonar = NewObject(TestActor, TEXT("TestImagingSonar")); + Sonar->SetupAttachment(Root); + Sonar->RegisterComponent(); + + const FString SonarParms = TEXT( + "{\"RangeMin\": 0.5, \"RangeMax\": 15, \"RangeBins\": 128, " + "\"Azimuth\": 120, \"AzimuthBins\": 64, " + "\"Elevation\": 20, \"ElevationBins\": 8}"); + + if (!AttachSensor(Sonar, TEXT("selftest_sonar"), SonarParms)) + { + UE_LOG(LogHolodeck, Error, TEXT("SelfTest: ImagingSonar attach failed")); + return; + } + + const int32 SonarBins = Sonar->GetDataBufferSize() / sizeof(float); + UE_LOG(LogHolodeck, Log, TEXT("SelfTest: ImagingSonar attached, buffer=%p bins=%d"), + Sonar->GetDataBuffer(), SonarBins); + + TWeakObjectPtr WeakSonar(Sonar); + FTimerHandle SonarHandle; + World->GetTimerManager().SetTimer(SonarHandle, [WeakSonar]() + { + UImagingSonar* S = WeakSonar.Get(); + if (S == nullptr) + { + UE_LOG(LogHolodeck, Warning, TEXT("SelfTest SONAR: sensor gone")); + return; + } + + const float* Data = static_cast(S->GetDataBuffer()); + const int32 Num = S->GetDataBufferSize() / sizeof(float); + if (Data == nullptr || Num <= 0) + { + UE_LOG(LogHolodeck, Error, TEXT("SelfTest SONAR: buffer null or empty (num=%d)"), Num); + return; + } + + int32 NonZero = 0; + float MinV = TNumericLimits::Max(); + float MaxV = TNumericLimits::Lowest(); + for (int32 i = 0; i < Num; ++i) + { + const float V = Data[i]; + if (V != 0.0f) + { + ++NonZero; + MinV = FMath::Min(MinV, V); + MaxV = FMath::Max(MaxV, V); + } + } + + if (NonZero > 0) + { + UE_LOG(LogHolodeck, Warning, + TEXT("SelfTest SONAR RESULT: bins=%d nonzero=%d min=%.3f max=%.3f => 声呐有回波"), + Num, NonZero, MinV, MaxV); + } + else + { + UE_LOG(LogHolodeck, Warning, + TEXT("SelfTest SONAR RESULT: bins=%d nonzero=0 => 全零,范围内可能没有障碍物或八叉树为空"), + Num); + } + }, 3.0f, false); +} + +void UUnderwaterSensorSubsystem::Deinitialize() +{ + if (Server != nullptr) + { + // 说明:Kill() 在 bIsRunning == false 时会直接 return,因此不会清空内部 + // 的 Memory 映射。这在本方案下是安全的——Server 是 UPROPERTY 持有的 + // UObject,被 GC 回收时其 Memory 成员(unique_ptr 容器)会正常析构, + // 命名共享内存随之释放。这里调用 Kill() 只是为了让日志与状态显式收尾。 + Server->Kill(); + Server = nullptr; + } + + Host = nullptr; + + UE_LOG(LogHolodeck, Log, TEXT("UnderwaterSensorSubsystem deinitialized")); + Super::Deinitialize(); +} + +bool UUnderwaterSensorSubsystem::AttachSensor(UHolodeckSensor* Sensor, const FString& AgentName, const FString& ParmsJson) +{ + if (Sensor == nullptr || !IsReady()) + { + UE_LOG(LogHolodeck, Warning, TEXT("AttachSensor: sensor or subsystem not ready")); + return false; + } + + // 参数必须先于 InitializeSensor:后者按 GetNumItems() 分配缓冲区 + if (!ParmsJson.IsEmpty()) + { + Sensor->ParseSensorParms(ParmsJson); + } + + Sensor->SetAgentAndController(Host, AgentName); + Sensor->InitializeSensor(); + + UE_LOG(LogHolodeck, Log, TEXT("AttachSensor: '%s' attached to agent '%s'"), + *Sensor->GetName(), *AgentName); + return true; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h index 6d16b0ede0..3b45eed9a3 100644 --- a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h @@ -79,6 +79,16 @@ class HOLODECK_API UHolodeckSensor : public USceneComponent UPROPERTY(EditAnywhere) bool bOn; + /** + * OpenHUTB 追加:读取传感器数据缓冲区。 + * 缓冲区由 UHolodeckServer::Malloc 分配,传感器在 TG_PostPhysics 阶段写入。 + * 返回 nullptr 表示尚未 InitializeSensor 或分配失败。 + */ + void* GetDataBuffer() const { return Buffer; } + + /** OpenHUTB 追加:缓冲区字节数(= GetNumItems() * GetItemSize()) */ + int32 GetDataBufferSize() { return GetNumItems() * GetItemSize(); } + protected: /** diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorHost.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorHost.h new file mode 100644 index 0000000000..016914262c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorHost.h @@ -0,0 +1,49 @@ +// MIT License — OpenHUTB 水下整合层 +// +// 最小化控制器:唯一职责是持有一个 UHolodeckServer, +// 让 HoloOcean 传感器能通过 Controller->GetServer()->Malloc(...) 拿到缓冲区。 +// +// 刻意不实现任何控制方案(AllocateBuffers / AddControlSchemes 等全部空实现), +// 因为 ROV 的运动由 URLab/MuJoCo 驱动,这里若也驱动 pawn 会互相抢控制权。 +// +// 对应方案文档:underwater/docs/notes/HoloOcean集成方案.md + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckPawnControllerInterface.h" + +#include "UnderwaterSensorHost.generated.h" + +/** + * AUnderwaterSensorHost + * 只提供 Server 的空控制器,用于挂载 HoloOcean 传感器。 + */ +UCLASS() +class HOLODECK_API AUnderwaterSensorHost : public AHolodeckPawnControllerInterface +{ + GENERATED_BODY() + +public: + AUnderwaterSensorHost(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * BeginPlay 在本 Actor 生成且场景 Actor 均已加载后触发, + * 是初始化八叉树的正确时机(UE4.26 的 UWorldSubsystem 没有对应钩子)。 + */ + virtual void BeginPlay() override; + + // --- 只实现这两个 --- + virtual void SetServer(UHolodeckServer* const ServerParam) override { Server = ServerParam; } + virtual UHolodeckServer* GetServer() override { return Server; } + + // --- 控制相关全部空实现:本控制器不驱动任何 pawn --- + virtual void AllocateBuffers(const FString& AgentName) override {} + virtual void AddControlSchemes() override {} + virtual void ExecuteTeleport() override {} + virtual void ExecuteSetState() override {} + +private: + UPROPERTY() + UHolodeckServer* Server = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorSubsystem.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorSubsystem.h new file mode 100644 index 0000000000..3913541b28 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/UnderwaterSensorSubsystem.h @@ -0,0 +1,98 @@ +// MIT License — OpenHUTB 水下整合层 +// +// 用 UWorldSubsystem 承载 HoloOcean 的服务器与传感器宿主, +// 从而完全绕开 AHolodeckGameMode / UHolodeckGameInstance, +// 避免与 Carla 的 ACarlaGameModeBase / UCarlaGameInstance 抢槽位。 +// +// 关键依据(见 docs/notes/HoloOcean集成方案.md): +// UHolodeckSensor::InitializeSensor() 只做一件事—— +// Controller->GetServer()->Malloc(key, size) +// 而 UHolodeckServer::Malloc 只分配共享内存,不检查 bIsRunning, +// 也不依赖 Start() 建立的信号量,因此无需启动服务器即可工作。 + +#pragma once + +#include "CoreMinimal.h" +#include "Subsystems/WorldSubsystem.h" + +#include "UnderwaterSensorSubsystem.generated.h" + +class UHolodeckServer; +class UHolodeckSensor; +class AUnderwaterSensorHost; + +/** + * UUnderwaterSensorSubsystem + * 在任意 World 中提供 HoloOcean 传感器所需的 Server 与 Host。 + */ +UCLASS() +class HOLODECK_API UUnderwaterSensorSubsystem : public UWorldSubsystem +{ + GENERATED_BODY() + +public: + virtual void Initialize(FSubsystemCollectionBase& Collection) override; + virtual void Deinitialize() override; + + /** + * 初始化八叉树。必须在场景 Actor 加载完成后调用。 + * UE4.26 的 UWorldSubsystem 没有 OnWorldBeginPlay 钩子, + * 因此由 AUnderwaterSensorHost::BeginPlay() 触发。幂等。 + */ + void EnsureOctreeReady(); + + /** 获取共享内存分配器 */ + UFUNCTION(BlueprintCallable, Category = "Underwater|Sensors") + UHolodeckServer* GetServer() const { return Server; } + + /** 获取传感器宿主控制器 */ + UFUNCTION(BlueprintCallable, Category = "Underwater|Sensors") + AUnderwaterSensorHost* GetHost() const { return Host; } + + /** + * 把一个 HoloOcean 传感器接入本子系统。 + * 会依次调用 ParseSensorParms(若给了参数)、SetAgentAndController、InitializeSensor。 + * 顺序不能颠倒——InitializeSensor 里会判 Controller 是否为空, + * 且它按 GetNumItems() 分配缓冲区,所以参数必须先于它生效。 + * + * @param Sensor 传感器组件(ImagingSonar / SidescanSonar / DVL 等) + * @param AgentName 代理名,用于拼接共享内存的 key + * @param ParmsJson 传感器参数 JSON,留空则使用类默认值 + */ + UFUNCTION(BlueprintCallable, Category = "Underwater|Sensors") + bool AttachSensor(UHolodeckSensor* Sensor, const FString& AgentName, const FString& ParmsJson = TEXT("")); + + /** 是否已成功初始化(Server 与 Host 均就绪) */ + UFUNCTION(BlueprintCallable, Category = "Underwater|Sensors") + bool IsReady() const { return Server != nullptr && Host != nullptr; } + + /** + * 自检:挂一个 LocationSensor 并读回数据,用于验证 + * 「Server → Malloc → Controller → Sensor Tick → Buffer」整条通路。 + * 由命令行开关 -UnderwaterSensorTest 触发,正常运行时不会执行。 + */ + void RunSensorSelfTest(); + + /** + * 八叉树作业区中心(UE 厘米单位)。 + * 声呐的射线加速结构只覆盖这个区域,超出范围的物体不会被探测到。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Sensors") + FVector EnvironmentCenter = FVector::ZeroVector; + + /** + * 八叉树作业区半尺寸(UE 厘米单位),默认 200m × 200m × 50m。 + * 调大能覆盖更大范围,但八叉树内存与构建时间按体积增长,需谨慎。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Sensors") + FVector EnvironmentExtent = FVector(20000.f, 20000.f, 5000.f); + +private: + UPROPERTY() + UHolodeckServer* Server = nullptr; + + UPROPERTY() + AUnderwaterSensorHost* Host = nullptr; + + bool bOctreeReady = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterFogActor.cpp b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterFogActor.cpp new file mode 100644 index 0000000000..eb4cedbce2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterFogActor.cpp @@ -0,0 +1,33 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "UnderwaterFogActor.h" + +#include "Components/ExponentialHeightFogComponent.h" + +AUnderwaterFogActor::AUnderwaterFogActor() +{ + PrimaryActorTick.bCanEverTick = false; + + Fog = CreateDefaultSubobject(TEXT("Fog")); + SetRootComponent(Fog); + + // 雾组件默认是 Movable,这里显式设一次,避免蓝图实例化时被改成 Static + Fog->SetMobility(EComponentMobility::Movable); +} + +void AUnderwaterFogActor::OnConstruction(const FTransform& Transform) +{ + Super::OnConstruction(Transform); + + if (!Fog) + { + return; + } + + Fog->SetFogDensity(FogDensity); + Fog->SetFogHeightFalloff(FogHeightFalloff); + Fog->SetFogInscatteringColor(FogColor); + Fog->SetFogMaxOpacity(FogMaxOpacity); + Fog->SetStartDistance(StartDistance); + Fog->SetFogCutoffDistance(FogCutoffDistance); +} diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPlaneActor.cpp b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPlaneActor.cpp new file mode 100644 index 0000000000..a3275efa35 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPlaneActor.cpp @@ -0,0 +1,45 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "UnderwaterPlaneActor.h" + +#include "Components/StaticMeshComponent.h" +#include "Engine/StaticMesh.h" +#include "UObject/ConstructorHelpers.h" + +AUnderwaterPlaneActor::AUnderwaterPlaneActor() +{ + PrimaryActorTick.bCanEverTick = false; + + PlaneMesh = CreateDefaultSubobject(TEXT("PlaneMesh")); + SetRootComponent(PlaneMesh); + + // 引擎自带的 1m×1m 平面 + static ConstructorHelpers::FObjectFinder PlaneAsset( + TEXT("/Engine/BasicShapes/Plane.Plane")); + if (PlaneAsset.Succeeded()) + { + PlaneMesh->SetStaticMesh(PlaneAsset.Object); + } + + // 平面本身不需要碰撞:海底/水面都是纯视觉,物理在 MuJoCo 里算。 + // 如果后续需要 ROV 撞海底,改成 QueryOnly 并加碰撞响应。 + PlaneMesh->SetCollisionEnabled(ECollisionEnabled::NoCollision); + PlaneMesh->SetCastShadow(false); + + ApplySize(); +} + +void AUnderwaterPlaneActor::ApplySize() +{ + if (!PlaneMesh) return; + + // 引擎 Plane 是 100cm × 100cm,所以缩放系数 = 米数 × 100 / 100 = 米数 + const float ScaleXY = FMath::Max(SizeMeters, 1.0f); + PlaneMesh->SetRelativeScale3D(FVector(ScaleXY, ScaleXY, 1.0f)); +} + +void AUnderwaterPlaneActor::OnConstruction(const FTransform& Transform) +{ + Super::OnConstruction(Transform); + ApplySize(); +} diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPostProcessActor.cpp b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPostProcessActor.cpp new file mode 100644 index 0000000000..ff1780da24 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterPostProcessActor.cpp @@ -0,0 +1,54 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "UnderwaterPostProcessActor.h" + +#include "Components/PostProcessComponent.h" +#include "Materials/MaterialInterface.h" +#include "Materials/Material.h" + +AUnderwaterPostProcessActor::AUnderwaterPostProcessActor() +{ + PrimaryActorTick.bCanEverTick = false; + + PostProcess = CreateDefaultSubobject(TEXT("PostProcess")); + SetRootComponent(PostProcess); + + // Unbound:整个场景都吃这个后处理。 + // M_Underwater_PP 内部按相机深度衰减,所以水面之上自然没有效果。 + PostProcess->bUnbound = true; + PostProcess->bEnabled = true; + PostProcess->Priority = 10.0f; +} + +void AUnderwaterPostProcessActor::OnConstruction(const FTransform& Transform) +{ + Super::OnConstruction(Transform); + + if (!bAutoAssignUnderwaterMaterial || !PostProcess) + { + return; + } + + // 已经有混合项就不再重复添加(OnConstruction 会被多次调用) + if (PostProcess->Settings.WeightedBlendables.Array.Num() > 0) + { + return; + } + + UMaterialInterface* UnderwaterMat = LoadObject( + nullptr, + TEXT("/OceanPlugin/Ocean/Materials/M_Underwater_PP.M_Underwater_PP")); + + if (!UnderwaterMat) + { + UE_LOG(LogTemp, Warning, + TEXT("[UnderwaterPostProcess] 找不到 M_Underwater_PP,后处理未挂载")); + return; + } + + PostProcess->Settings.WeightedBlendables.Array.Add( + FWeightedBlendable(1.0f, UnderwaterMat)); + + UE_LOG(LogTemp, Log, + TEXT("[UnderwaterPostProcess] 已挂载 M_Underwater_PP(权重 1.0)")); +} diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterScene.cpp b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterScene.cpp new file mode 100644 index 0000000000..71a921735c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Private/UnderwaterScene.cpp @@ -0,0 +1,5 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Modules/ModuleManager.h" + +IMPLEMENT_MODULE(FDefaultModuleImpl, UnderwaterScene); diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterFogActor.h b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterFogActor.h new file mode 100644 index 0000000000..b3d96a636d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterFogActor.h @@ -0,0 +1,68 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "UnderwaterFogActor.generated.h" + +class UExponentialHeightFogComponent; + +/** + * 水下雾。 + * + * 为什么需要它 + * ------------ + * 之前的水下氛围由 OceanPlugin 的 M_Underwater_PP 提供,但那个材质内部用 + * UnderwaterLightDepth / UnderwaterNoLightDepth 两个硬深度阈值判断「是否在水下」, + * 相机高度落在阈值附近时判断会在 0/1 之间反复翻转,整屏以约 3.3 秒周期方波闪烁。 + * 参数表里没有可关闭的开关,只能弃用。 + * + * 本 Actor 用引擎自带的 UExponentialHeightFogComponent 提供**真正的距离雾**: + * - 纯静态参数,不随时间变化,不存在闪烁可能 + * - 按距离指数衰减,远处比近处更「雾」,这正是水下该有的观感 + * - 不依赖任何第三方材质 + * + * 为什么不用 AExponentialHeightFog + * -------------------------------- + * 和 APostProcessVolume 一样,它不是 Blueprintable,无头模式下建不出蓝图子类, + * 而 spawn_actor_from_class 在无头模式会挂起。所以这里自建一个可蓝图化的载体。 + */ +UCLASS(Blueprintable) +class UNDERWATERSCENE_API AUnderwaterFogActor : public AActor +{ + GENERATED_BODY() + +public: + AUnderwaterFogActor(); + + UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "Underwater|Fog") + UExponentialHeightFogComponent* Fog; + + /** 雾密度。越大越浑,0.02 约等于能见度数十米。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + float FogDensity = 0.02f; + + /** 高度衰减。水下雾基本是均匀的,取小值即可。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + float FogHeightFalloff = 0.2f; + + /** 雾色(线性空间)。偏青蓝,模拟水体对红光的吸收。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + FLinearColor FogColor = FLinearColor(0.02f, 0.12f, 0.16f, 1.0f); + + /** 最大不透明度,1.0 表示远处完全被雾吃掉。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + float FogMaxOpacity = 1.0f; + + /** 起始距离(cm)。留一点余量避免近处糊成一片。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + float StartDistance = 0.0f; + + /** 截止距离(cm)。0 表示不限制。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Fog") + float FogCutoffDistance = 0.0f; + +protected: + virtual void OnConstruction(const FTransform& Transform) override; +}; diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPlaneActor.h b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPlaneActor.h new file mode 100644 index 0000000000..f4f9127946 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPlaneActor.h @@ -0,0 +1,55 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "UnderwaterPlaneActor.generated.h" + +class UStaticMeshComponent; + +/** + * 可配置的平面 Actor —— 用来铺海底和水面。 + * + * 为什么需要它: + * UE4.26 的 Python 在 -run=pythonscript 无头模式下,spawn_actor_from_class 和 + * spawn_actor_from_object(C++ 类) 都会挂起(等编辑器视口),只有传「蓝图对象」 + * 才能成功放置。而 Landscape / StaticMeshActor 这类 C++ 类没法直接变成蓝图 + * (Landscape 不是 Blueprintable)。 + * + * 所以这里提供一个 Blueprintable 的 C++ Actor,在构造函数里建好 StaticMeshComponent, + * 再由 Python 创建一个蓝图子类(可以改网格/缩放/材质),最后用 + * spawn_actor_from_object 放置。 + * + * 用法: + * 1. Python 里用 BlueprintFactory(parent_class=UnderwaterPlaneActor) 建蓝图 + * 2. 改蓝图的 Mesh / Scale / Material 属性 + * 3. spawn_actor_from_object 放到场景里 + */ +UCLASS(Blueprintable) +class UNDERWATERSCENE_API AUnderwaterPlaneActor : public AActor +{ + GENERATED_BODY() + +public: + AUnderwaterPlaneActor(); + + /** 平面网格组件。默认用引擎的 1m×1m Plane,靠 Scale 放大到需要的尺寸。 */ + UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "Underwater|Plane") + UStaticMeshComponent* PlaneMesh; + + /** + * 平面边长(米)。 + * 2000 表示 2000m × 2000m,足以覆盖 Town10HD(约 900m × 1500m)。 + * 在蓝图里改这个值即可,构造函数会把网格缩放算好。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|Plane") + float SizeMeters = 2000.0f; + + /** 在蓝图里改完 SizeMeters 后调用,重新计算网格缩放。 */ + UFUNCTION(BlueprintCallable, CallInEditor, Category = "Underwater|Plane") + void ApplySize(); + +protected: + virtual void OnConstruction(const FTransform& Transform) override; +}; diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPostProcessActor.h b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPostProcessActor.h new file mode 100644 index 0000000000..c7f630758e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/Public/UnderwaterPostProcessActor.h @@ -0,0 +1,41 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "UnderwaterPostProcessActor.generated.h" + +class UPostProcessComponent; + +/** + * 水下后处理 Actor。 + * + * 为什么不用 APostProcessVolume: + * AVolume/ABrush 不是 Blueprintable,Python 的 BlueprintFactory 建不出子类; + * 而无头模式下又只能放置蓝图对象。所以这里用「普通 Actor + UPostProcessComponent」 + * 自己拼一个等效的、可蓝图化的后处理载体。 + * + * 行为: + * - PostProcessComponent 设为 Unbound(全场景生效) + * - 构造时把 OceanPlugin 的 M_Underwater_PP 加进 Blendables(权重 1.0) + * - 材质内部按相机深度衰减,所以水面之上看不到效果,入水后才显现 + */ +UCLASS(Blueprintable) +class UNDERWATERSCENE_API AUnderwaterPostProcessActor : public AActor +{ + GENERATED_BODY() + +public: + AUnderwaterPostProcessActor(); + + UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "Underwater|PostProcess") + UPostProcessComponent* PostProcess; + + /** 是否在构造时自动挂上 M_Underwater_PP。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Underwater|PostProcess") + bool bAutoAssignUnderwaterMaterial = true; + +protected: + virtual void OnConstruction(const FTransform& Transform) override; +}; diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/UnderwaterScene.Build.cs b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/UnderwaterScene.Build.cs new file mode 100644 index 0000000000..b9feca9d49 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/Source/UnderwaterScene/UnderwaterScene.Build.cs @@ -0,0 +1,18 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +using UnrealBuildTool; + +public class UnderwaterScene : ModuleRules +{ + public UnderwaterScene(ReadOnlyTargetRules Target) : base(Target) + { + PCHUsage = PCHUsageMode.UseExplicitOrSharedPCHs; + + PublicDependencyModuleNames.AddRange(new string[] + { + "Core", + "CoreUObject", + "Engine" + }); + } +} diff --git a/Unreal/CarlaUE4/Plugins/UnderwaterScene/UnderwaterScene.uplugin b/Unreal/CarlaUE4/Plugins/UnderwaterScene/UnderwaterScene.uplugin new file mode 100644 index 0000000000..3f3b4cd275 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnderwaterScene/UnderwaterScene.uplugin @@ -0,0 +1,21 @@ +{ + "FileVersion": 3, + "Version": 1, + "VersionName": "1.0", + "FriendlyName": "Underwater Scene", + "Description": "水下场景搭建辅助:可配置的平面 Actor(海底/水面),供 Python 通过蓝图放置", + "Category": "Simulation", + "CreatedBy": "OpenHUTB", + "CreatedByURL": "https://github.com/OpenHUTB", + "CanContainContent": true, + "IsBetaVersion": false, + "IsExperimentalVersion": true, + "Installed": true, + "Modules": [ + { + "Name": "UnderwaterScene", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ] +} diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/Materials/M_MuJoCo_Master.uasset b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/Materials/M_MuJoCo_Master.uasset new file mode 100644 index 0000000000..1e7f5858ae Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/Materials/M_MuJoCo_Master.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/arm_inc.xml b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/arm_inc.xml new file mode 100644 index 0000000000..9211e6f3f2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/arm_inc.xml @@ -0,0 +1,12 @@ + + + + + + + + + + diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/deps_inc.xml b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/deps_inc.xml new file mode 100644 index 0000000000..b947685281 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/deps_inc.xml @@ -0,0 +1,5 @@ + + + + diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/parent.xml b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/parent.xml new file mode 100644 index 0000000000..f17d23332a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/parent.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/scene_inc.xml b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/scene_inc.xml new file mode 100644 index 0000000000..7aca77a0ec --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/scene_inc.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.glb b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.glb new file mode 100644 index 0000000000..6f7f1b9058 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.glb differ diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.stl b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.stl new file mode 100644 index 0000000000..ab35cdf764 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Content/TestData/issue72_includes/tabletop.stl differ diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Components/Geometry/MjGeom.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Components/Geometry/MjGeom.cpp index fbeb25cacb..25335da8e0 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Components/Geometry/MjGeom.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Components/Geometry/MjGeom.cpp @@ -554,6 +554,7 @@ void UMjGeom::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) MjName = UniqueName; TArray ExtraMeshNames; + UStaticMeshComponent* MeshSourceSMC = nullptr; if (Type == EMjGeomType::Mesh) { @@ -610,6 +611,7 @@ void UMjGeom::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) { if (UStaticMeshComponent* SMC = Cast(Child)) { + MeshSourceSMC = SMC; TArray AssetNames = Wrapper.PrepareMeshForMuJoCo(SMC, bComplexMeshRequired); if (AssetNames.Num() > 0) { @@ -622,6 +624,54 @@ void UMjGeom::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) } } + // OpenHUTB 追加(2026-11-07):打包版没有网格 CPU 数据时,PrepareMeshForMuJoCo 会跳过导出, + // MeshName 可能为空、也可能仍是 XML 里的名字但对应 mesh 从未注册进 spec。 + // 只要 spec 里找不到这个 mesh,就退化成一个包围盒 geom: + // 视觉仍由 UE 渲染真实网格,物理用盒子近似,保证打包版 mj_compile 不失败。 + const bool bMeshRegistered = !MeshName.IsEmpty() && + (mjs_findElement(Wrapper.Spec, mjOBJ_MESH, TCHAR_TO_UTF8(*MeshName)) != nullptr); + if (Type == EMjGeomType::Mesh && !bMeshRegistered) + { + if (!MeshSourceSMC) + { + TArray FallbackChildren; + GetChildrenComponents(true, FallbackChildren); + for (USceneComponent* Child : FallbackChildren) + { + if (UStaticMeshComponent* SMC = Cast(Child)) + { + MeshSourceSMC = SMC; + break; + } + } + } + + FVector ExtentM(0.01, 0.01, 0.01); + bool bHasExtent = false; + if (MeshSourceSMC) + { + if (UStaticMesh* SourceMesh = MeshSourceSMC->GetStaticMesh()) + { + const FVector LocalExtentM = SourceMesh->GetBoundingBox().GetExtent() * 0.01; // cm -> m + if (LocalExtentM.GetMin() > 1e-4 && LocalExtentM.GetMax() < 100.0) + { + ExtentM = LocalExtentM; + bHasExtent = true; + } + } + } + + Type = EMjGeomType::Box; + bOverride_Type = true; + Size = ExtentM; + bOverride_Size = true; + SizeParamsCount = 3; + UE_LOG(LogURLab, Warning, + TEXT("[MjGeom] '%s' 网格 '%s' 未注册(打包版无 CPU 数据),退化为包围盒 (%.3f, %.3f, %.3f) m%s"), + *GetName(), *MeshName, ExtentM.X, ExtentM.Y, ExtentM.Z, bHasExtent ? TEXT("") : TEXT("(兜底尺寸)")); + MeshName.Empty(); + } + ExportTo(geom, SpecDef); // 来自复杂分解的额外网格名称 diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp index 8379153166..69d1c237da 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp @@ -378,9 +378,16 @@ void UMjPhysicsEngine::RunMujocoAsync() } } - for (const FPhysicsCallback& Cb : PreStepCallbacks) + // 回调执行期间可能注册/清空回调,直接 ranged-for 会触发 + // "Array has changed during ranged-for iteration",这里先记下数量再按下标取。 + const int32 NumPreStep = PreStepCallbacks.Num(); + for (int32 i = 0; i < NumPreStep; ++i) { - Cb(m_model, m_data); + if (!PreStepCallbacks.IsValidIndex(i)) + { + break; + } + PreStepCallbacks[i](m_model, m_data); } for (AMjArticulation* Art : m_articulations) @@ -396,9 +403,14 @@ void UMjPhysicsEngine::RunMujocoAsync() mj_step(m_model, m_data); } - for (const FPhysicsCallback& Cb : PostStepCallbacks) + const int32 NumPostStep = PostStepCallbacks.Num(); + for (int32 i = 0; i < NumPostStep; ++i) { - Cb(m_model, m_data); + if (!PostStepCallbacks.IsValidIndex(i)) + { + break; + } + PostStepCallbacks[i](m_model, m_data); } if (OnPostStep) @@ -407,12 +419,20 @@ void UMjPhysicsEngine::RunMujocoAsync() } } // FScopeLock 在这里释放 - // 在小时间步长下自旋等待精确计时 + // 在小时间步长下等待精确计时 + // + // OpenHUTB 修改(2026-11-05):原来是 Sleep(0) 纯自旋,2ms 步长下会 + // 吃满一个 CPU 核。改为「先睡掉剩余时间的 60%,再短自旋收尾」, + // 兼顾计时精度与 CPU 占用。 const float SpeedFactor = FMath::Clamp(SimSpeedPercent, 5.0f, 100.0f) / 100.0f; const double TargetTime = LoopStartTime + (TargetInterval / SpeedFactor); + double Remaining = TargetTime - FPlatformTime::Seconds(); + if (Remaining > 0.002) { + // 留 2ms 给自旋收尾,其余时间真正休眠 + FPlatformProcess::Sleep(static_cast(Remaining - 0.002)); + } while (FPlatformTime::Seconds() < TargetTime) { - // FPlatformProcess::YieldThread(); FPlatformProcess::Sleep(0.0f); } } diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/Spec/MjSpecWrapper.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/Spec/MjSpecWrapper.cpp index 5f10d19dff..fc2ec5ae5f 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/Spec/MjSpecWrapper.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Core/Spec/MjSpecWrapper.cpp @@ -229,13 +229,11 @@ TArray FMujocoSpecWrapper::PrepareMeshForMuJoCo(UStaticMeshComponent* S if (!SMC || !SMC->GetStaticMesh()) return ResultNames; UStaticMesh* Mesh = SMC->GetStaticMesh(); - UBodySetup* BodySetup = nullptr; // Mesh->GetBodySetup(); - // if (!BodySetup || BodySetup->TriMeshGeometries.Num() == 0) return ResultNames; - if (!BodySetup) return ResultNames; FString MeshType = bComplexMeshRequired ? "Complex" : "Simple"; - UStaticMesh* StaticMesh = Cast(BodySetup->GetOuter()); - FString BaseAssetName = StaticMesh ? StaticMesh->GetName() : SMC->GetName(); + // OpenHUTB 修改(2026-11-04):原为 Cast(BodySetup->GetOuter()), + // 但 BodySetup 在 UE4.26 下已被置空(见下),这里直接用 Mesh 本身。 + FString BaseAssetName = Mesh ? Mesh->GetName() : SMC->GetName(); FVector Scale = SMC->GetComponentScale(); FString AssetName = BaseAssetName; @@ -288,6 +286,100 @@ TArray FMujocoSpecWrapper::PrepareMeshForMuJoCo(UStaticMeshComponent* S } } + // ======================================================================== + // OpenHUTB 追加(2026-11-04):UE4.26 版网格导出 + // + // 上游这段实现依赖 UE5 的 Chaos API(UBodySetup::TriMeshGeometries、 + // TriGeom.GetReference()->Particles()),在 UE4.26 下不存在,因此整段被注释掉, + // 导致任何含 mesh geom 的模型 Compile() 都报 "mesh not found"。 + // + // 这里用 UE4.26 的 FStaticMeshLODResources 重新实现:从渲染数据读顶点与索引, + // 写 OBJ 后注册进 spec。坐标转换沿用上游 SaveMeshAsOBJSimple 的约定: + // UE (cm, 左手, Z-up) -> MuJoCo (m, 右手, Z-up): (x/100, -y/100, z/100),绕序翻转 + // ======================================================================== + // UE4.26: RenderData 是 UStaticMesh 的公开成员(引擎内部同样这样访问) + if (!Mesh->HasValidRenderData() || !Mesh->RenderData.IsValid() + || Mesh->RenderData->LODResources.Num() == 0) + { + UE_LOG(LogURLabWrapper, Warning, + TEXT("PrepareMeshForMuJoCo: '%s' 没有可用的渲染数据"), *BaseAssetName); + return ResultNames; + } + + const FStaticMeshLODResources& LOD = Mesh->RenderData->LODResources[0]; + const FPositionVertexBuffer& PositionBuffer = LOD.VertexBuffers.PositionVertexBuffer; + const FRawStaticIndexBuffer& IndexBuffer = LOD.IndexBuffer; + + const uint32 NumVerts = PositionBuffer.GetNumVertices(); + const uint32 NumIndices = IndexBuffer.GetNumIndices(); + + // OpenHUTB 修复(2026-11-07):打包版(cooked)会把 IndexStorage 的 CPU 副本剥离, + // 但 CachedNumIndices 仍保留真实计数。此时调用 GetIndex() 会触发 + // Assertion failed: At >= 0 && At < (uint32)IndexStorage.Num() + // [RawIndexBuffer.h:177] + // 在 Shipping 里表现为启动 60~70 秒后 Fatal error(退出码 3)直接退出。 + // 用 GetIndexDataSize()(CPU 字节数)判断数据是否真的可用。 + if (IndexBuffer.GetIndexDataSize() <= 0) + { + UE_LOG(LogURLabWrapper, Warning, + TEXT("PrepareMeshForMuJoCo: '%s' 的索引缓冲没有 CPU 副本(cooked 包常见),跳过网格导出"), + *BaseAssetName); + return ResultNames; + } + const uint32 IndexStride = IndexBuffer.Is32Bit() ? 4u : 2u; + if (NumIndices == 0 || + (uint64)NumIndices * (uint64)IndexStride > (uint64)IndexBuffer.GetIndexDataSize()) + { + UE_LOG(LogURLabWrapper, Warning, + TEXT("PrepareMeshForMuJoCo: '%s' 索引数据不完整(count=%u, stride=%u, bytes=%d),跳过网格导出"), + *BaseAssetName, NumIndices, IndexStride, IndexBuffer.GetIndexDataSize()); + return ResultNames; + } + + if (NumVerts == 0 || NumIndices < 3) + { + UE_LOG(LogURLabWrapper, Warning, + TEXT("PrepareMeshForMuJoCo: '%s' 顶点或索引为空 (v=%u i=%u)"), + *BaseAssetName, NumVerts, NumIndices); + return ResultNames; + } + + FString OutputString; + OutputString.Reserve(NumVerts * 40 + NumIndices * 12); + + for (uint32 i = 0; i < NumVerts; ++i) + { + const FVector V = PositionBuffer.VertexPosition(i); + OutputString += FString::Printf(TEXT("v %f %f %f\n"), + V.X / 100.0, -V.Y / 100.0, V.Z / 100.0); + } + for (uint32 j = 0; j + 2 < NumIndices; j += 3) + { + const uint32 A = IndexBuffer.GetIndex(j); + const uint32 B = IndexBuffer.GetIndex(j + 1); + const uint32 C = IndexBuffer.GetIndex(j + 2); + // 绕序翻转,匹配 MuJoCo 的右手系 + OutputString += FString::Printf(TEXT("f %u %u %u\n"), C + 1, B + 1, A + 1); + } + + IFileManager::Get().MakeDirectory(*FPaths::GetPath(FullFilePath), true); + if (!FFileHelper::SaveStringToFile(OutputString, *FullFilePath)) + { + UE_LOG(LogURLabWrapper, Error, + TEXT("PrepareMeshForMuJoCo: 写出 OBJ 失败 '%s'"), *FullFilePath); + return ResultNames; + } + + // 顶点已按 cm->m 换算,这里不再额外施加组件缩放,避免二次缩放 + ResultNames.Add(AddMeshAsset(AssetName, FullFilePath, FVector::OneVector)); + + UE_LOG(LogURLabWrapper, Log, + TEXT("PrepareMeshForMuJoCo: '%s' -> mesh '%s' (%u 顶点, %u 三角面)"), + *BaseAssetName, *AssetName, NumVerts, NumIndices / 3); + + return ResultNames; + + // 以下为上游 UE5 实现(保留备查,UE4.26 下不可用) const int32 GeometryIndex = 0; // // auto& TriGeom; // BodySetup->TriMeshGeometries[GeometryIndex]; // auto& Vertices = TriGeom.GetReference()->Particles().X(); diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Input/MjPerturbation.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Input/MjPerturbation.cpp index 69cb54f28a..4cdc6c15a9 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Input/MjPerturbation.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Private/MuJoCo/Input/MjPerturbation.cpp @@ -79,9 +79,20 @@ void UMjPerturbation::BeginPlay() if (bRunning) { - // Zero xfrc on every body: otherwise the last force stays - // latched after release and the body drifts indefinitely. - mju_zero(d->xfrc_applied, 6 * m->nbody); + // OpenHUTB 修改(2026-11-06): + // 原来无条件 mju_zero(d->xfrc_applied, 6 * m->nbody),把所有 body + // 的外力每步清空。这对"只用鼠标拖拽"的场景是对的(否则松手后力 + // 会一直留着),但 xfrc_applied 是 MuJoCo 唯一的外力公共入口, + // 无条件清空会抹掉其他系统施加的浮力/推进器推力/洋流拖曳, + // 使这些力完全失效。 + // + // 改成只在**真正拖拽时**才清,而且只清它自己拖的那个 body。 + // 拖拽行为不变;不拖的时候完全不动 xfrc_applied, + // 让其他系统(浮力/推进器/洋流)施加的外力得以保留。 + if (Perturb.active && Perturb.select > 0 && Perturb.select < m->nbody) + { + mju_zero(d->xfrc_applied + 6 * Perturb.select, 6); + } mjv_applyPerturbPose(m, d, &Perturb, 0); mjv_applyPerturbForce(m, d, &Perturb); } diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h index 4f0270e9cd..50321ae391 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h @@ -129,6 +129,12 @@ class URLAB_API UMjBody : public UMjComponent // --- Runtime World-State Accessors (BlueprintCallable) --- + // 注:绑定状态用基类 UMjComponent::IsBound() 即可 + // (实现为 m_Model && m_Data && m_ID >= 0)。 + // 曾尝试在此处重定义 IsBound(),UHT 报错: + // "Override of UFUNCTION in parent class (MjComponent) cannot have a UFUNCTION() declaration above it" + // 因此不再重复声明。 + /** @brief Gets this body's world position in Unreal Engine coordinates (cm). */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|Runtime") FVector GetWorldPosition() const; diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/URLab.Build.cs b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/URLab.Build.cs index 98ea3b0bc1..65a466bdf1 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/URLab.Build.cs +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLab/URLab.Build.cs @@ -74,6 +74,11 @@ public URLab(ReadOnlyTargetRules Target) : base(Target) // Add dynamically loaded modules here }); + // 显式声明公共头目录。部分引擎构建(CI 使用的 Build\engine)默认关闭 + // legacy public include paths,依赖模块(如 AirSim 的 RovMuJoCoBridge.cpp) + // 会找不到 MuJoCo/Core/*.h。 + PublicIncludePaths.Add(Path.Combine(ModuleDirectory, "Public")); + if (Target.Platform == UnrealTargetPlatform.Win64) { PublicSystemLibraries.AddRange(new string[] diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoMeshImporter.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoMeshImporter.cpp index cd56e5d92d..795f23d7f0 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoMeshImporter.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoMeshImporter.cpp @@ -631,10 +631,30 @@ UMaterialInstanceConstant* UMujocoGenerationAction::CreateMaterialInstance( FAssetRegistryModule::AssetCreated(MaterialInstance); FString PackageFileName = FPackageName::LongPackageNameToFilename(PackageName, FPackageName::GetAssetPackageExtension()); - FSavePackageArgs SaveArgs; - SaveArgs.TopLevelFlags = RF_Public | RF_Standalone; - SaveArgs.SaveFlags = SAVE_NoError; - // UPackage::SavePackage(Package, MaterialInstance, *PackageFileName, SaveArgs); + + // OpenHUTB 修改(2026-11-05):原来这行被注释掉了,导致材质实例只在内存里创建、 + // 注册进资产注册表,但从不写盘。编辑器一关就丢,蓝图里的引用变成悬空, + // 模型渲染成灰白(日志报 "Failed to load .../MI_xxx: Can't find file")。 + // + // 注意 UE4.26 的 UPackage::SavePackage 签名是逐个参数,没有 FSavePackageArgs 重载: + // SavePackage(InOuter, InAsset, TopLevelFlags, Filename, Error, ConcurrencyObject, + // bForceByteSwapping, bWarnOfLongFilename, SaveFlags, ...) + const bool bSaved = UPackage::SavePackage( + Package, + MaterialInstance, + RF_Public | RF_Standalone, + *PackageFileName, + GError, + nullptr, + false, + true, + SAVE_NoError); + + if (!bSaved) + { + UE_LOG(LogURLabEditor, Error, + TEXT("Failed to save material instance to disk: %s"), *PackageFileName); + } UE_LOG(LogURLabEditor, Log, TEXT("Successfully created material instance: %s"), *InstanceName); return MaterialInstance; diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoXmlParser.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoXmlParser.cpp index 149e5ca9dc..a49d85bf9c 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoXmlParser.cpp +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/MujocoXmlParser.cpp @@ -1127,6 +1127,20 @@ void UMujocoGenerationAction::ParseAssetsRecursive(const FXmlNode* Node, const F { FString TexName = Node->GetAttribute(TEXT("name")); FString TexFile = Node->GetAttribute(TEXT("file")); + + // OpenHUTB 追加(2026-11-04): + // MuJoCo 支持 builtin 纹理(如 builtin="checker"),这类纹理由引擎程序化生成, + // 磁盘上没有对应文件。若照常当作文件加载会报 + // "Failed to load texture file: .../tex_seafloor" + // 并让自动化测试整体判为失败。这里直接跳过。 + FString TexBuiltin = Node->GetAttribute(TEXT("builtin")); + if (!TexBuiltin.IsEmpty()) + { + UE_LOG(LogURLabEditor, Log, + TEXT("Skipping builtin texture '%s' (builtin=%s)"), *TexName, *TexBuiltin); + return; + } + if (TexFile.IsEmpty()) TexFile = TexName; if (!TexFile.IsEmpty()) diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/Tests/MjUnderwaterImportTests.cpp b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/Tests/MjUnderwaterImportTests.cpp new file mode 100644 index 0000000000..77c36ef8c5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/Private/Tests/MjUnderwaterImportTests.cpp @@ -0,0 +1,174 @@ +// OpenHUTB 追加(2026-11-04) +// +// 验证水下机器人 ROV 模型能否通过 URLab 的 MJCF 导入管线正确导入。 +// 目的:证实「导入器不支持 .stl,导致 mesh 退化为立方体」这一判断。 +// +// 用法: +// UE4Editor.exe -ExecCmds="Automation RunTests URLab.Import.UnderwaterRov; Quit" -unattended +// +// 通过环境变量 ROV_XML_PATH 指定要导入的 XML,默认用修复后的 OBJ 版本: +// set ROV_XML_PATH=...\rov_model_fixed\underwater_rov_with_arm.xml (期望:通过) +// set ROV_XML_PATH=...\models\underwater_rov_with_arm.xml (期望:mesh 缺失) +// +// 参考 docs/notes/ROV模型导入问题定位.md + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "HAL/PlatformMisc.h" + +#include "Tests/MjTestHelpers.h" +#include "MuJoCo/Components/Geometry/MjGeom.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" + +#include "mujoco/mujoco.h" + +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjImportUnderwaterRov, + "URLab.Import.UnderwaterRov", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjImportUnderwaterRov::RunTest(const FString& Parameters) +{ + // 允许通过环境变量切换待测模型,便于对比 STL 与 OBJ 两个版本 + FString XmlPath = FPlatformMisc::GetEnvironmentVariable(TEXT("ROV_XML_PATH")); + if (XmlPath.IsEmpty()) + { + XmlPath = TEXT("E:/Projects/underwater/out/rov_model_fixed/underwater_rov_with_arm.xml"); + } + + AddInfo(FString::Printf(TEXT("Importing: %s"), *XmlPath)); + + if (!FPaths::FileExists(XmlPath)) + { + AddWarning(FString::Printf(TEXT("跳过:XML 不存在 %s"), *XmlPath)); + return true; + } + + FMjXmlImportSession S; + if (!S.InitFromFile(XmlPath)) + { + AddError(FString::Printf(TEXT("InitFromFile 失败:%s"), *S.LastError)); + S.Cleanup(); + return false; + } + + AddInfo(FString::Printf( + TEXT("原生 MuJoCo 基准:bodies=%d joints=%d actuators=%d geoms=%d"), + (int32)S.NativeBodyCount, (int32)S.NativeJointCount, + (int32)S.NativeActuatorCount, (int32)S.NativeGeomCount)); + + const int32 BPBodies = S.CountTemplates(); + const int32 BPJoints = S.CountTemplates(); + const int32 BPActuators = S.CountTemplates(); + const int32 BPGeoms = S.CountTemplates(); + + AddInfo(FString::Printf( + TEXT("蓝图组件:bodies=%d joints=%d actuators=%d geoms=%d(含 defaults)"), + BPBodies, BPJoints, BPActuators, BPGeoms)); + + TestTrue(TEXT("蓝图应有 body 组件"), BPBodies >= 1); + TestTrue(TEXT("蓝图应有 geom 组件"), BPGeoms >= 1); + + if (!S.Compile()) + { + AddError(FString::Printf(TEXT("Compile 失败:%s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Model(); + if (M == nullptr) + { + AddError(TEXT("编译后模型为空")); + S.Cleanup(); + return false; + } + + AddInfo(FString::Printf( + TEXT("URLab 编译结果:bodies=%d joints=%d actuators=%d geoms=%d meshes=%d"), + (int32)M->nbody, (int32)M->njnt, (int32)M->nu, (int32)M->ngeom, (int32)M->nmesh)); + + // 结构一致性:应与原生 MuJoCo 一致 + TestEqual(TEXT("body 数应与原生一致"), (int32)M->nbody, (int32)S.NativeBodyCount); + TestEqual(TEXT("joint 数应与原生一致"), (int32)M->njnt, (int32)S.NativeJointCount); + TestEqual(TEXT("actuator 数应与原生一致"), (int32)M->nu, (int32)S.NativeActuatorCount); + TestEqual(TEXT("geom 数应与原生一致"), (int32)M->ngeom, (int32)S.NativeGeomCount); + + // 关键判据:mesh 是否被正确加载 + // ROV 模型有 28 个 mesh;若导入器无法解析文件,nmesh 会明显小于 28 + AddInfo(FString::Printf(TEXT("mesh 数量:%d(期望 28)"), (int32)M->nmesh)); + TestTrue(TEXT("应加载到 mesh(期望 28 个)"), (int32)M->nmesh >= 20); + + // 能步进即说明模型可用 + mjData* D = S.Data(); + if (D != nullptr) + { + mj_step(M, D); + mj_step(M, D); + AddInfo(TEXT("模型可正常步进 2 次")); + + // ------------------------------------------------------------------ + // 抓取物理连通性验证 + // 驱动夹爪执行器闭合,步进若干帧,确认: + // 1) 夹爪指关节确实在动(qpos 变化) + // 2) 场景中出现接触(说明碰撞体在工作) + // 注意:模型本身不含可抓取试件,这里只验证物理链路,不做抓取成功率统计。 + // ------------------------------------------------------------------ + // URLab 会给 spec 里的元素名加蓝图前缀(ImportTestArt_xxx_), + // 所以不能按原名查找,改为后缀匹配。 + auto FindByNameSuffix = [](const mjModel* Model, mjtObj Type, const FString& Suffix) -> int32 + { + const int32 Count = (Type == mjOBJ_ACTUATOR) ? (int32)Model->nu + : (Type == mjOBJ_JOINT) ? (int32)Model->njnt : 0; + for (int32 i = 0; i < Count; ++i) + { + const char* Name = mj_id2name(Model, Type, i); + if (Name && FString(ANSI_TO_TCHAR(Name)).EndsWith(Suffix)) + { + return i; + } + } + return -1; + }; + + const int32 ActId = FindByNameSuffix(M, mjOBJ_ACTUATOR, TEXT("gripper_fingers_actuator")); + if (ActId >= 0) + { + // 记录初始指关节位置 + const int32 JntL = FindByNameSuffix(M, mjOBJ_JOINT, TEXT("gripper_left_driver_joint")); + const int32 JntR = FindByNameSuffix(M, mjOBJ_JOINT, TEXT("gripper_right_driver_joint")); + const double Q0L = (JntL >= 0) ? D->qpos[M->jnt_qposadr[JntL]] : 0.0; + const double Q0R = (JntR >= 0) ? D->qpos[M->jnt_qposadr[JntR]] : 0.0; + + // 闭合夹爪(ctrlrange 是 0..255,取上限) + D->ctrl[ActId] = 255.0; + for (int32 i = 0; i < 500; ++i) + { + mj_step(M, D); + } + + const double Q1L = (JntL >= 0) ? D->qpos[M->jnt_qposadr[JntL]] : 0.0; + const double Q1R = (JntR >= 0) ? D->qpos[M->jnt_qposadr[JntR]] : 0.0; + const double DeltaL = FMath::Abs(Q1L - Q0L); + const double DeltaR = FMath::Abs(Q1R - Q0R); + + AddInfo(FString::Printf( + TEXT("夹爪闭合 500 帧后:左指位移 %.4f rad,右指位移 %.4f rad,接触数 %d"), + DeltaL, DeltaR, (int32)D->ncon)); + + TestTrue(TEXT("夹爪执行器应能驱动指关节运动"), + (DeltaL + DeltaR) > 1e-4); + TestTrue(TEXT("闭合过程中应产生接触"), (int32)D->ncon > 0); + } + else + { + AddWarning(TEXT("未找到 gripper_fingers_actuator,跳过抓取物理验证")); + } + } + + S.Cleanup(); + return true; +} diff --git a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/URLabEditor.Build.cs b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/URLabEditor.Build.cs index 663c0a450a..5a8d15a9f7 100644 --- a/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/URLabEditor.Build.cs +++ b/Unreal/CarlaUE4/Plugins/UnrealRoboticsLab/Source/URLabEditor/URLabEditor.Build.cs @@ -34,5 +34,9 @@ public URLabEditor(ReadOnlyTargetRules Target) : base(Target) "DesktopPlatform", "Kismet" }); + + // UE4.26 的 Blutility 模块把 BlutilityMenuExtensions.h 放在 Private/ 下, + // 需要手动补充私有 include 路径。 + PrivateIncludePaths.Add(Path.Combine(EngineDirectory, "Source", "Editor", "Blutility", "Private")); } } diff --git a/Unreal/CarlaUE4/Plugins/setup_ocean_plugin.ps1 b/Unreal/CarlaUE4/Plugins/setup_ocean_plugin.ps1 new file mode 100644 index 0000000000..676c1dbd5a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/setup_ocean_plugin.ps1 @@ -0,0 +1,123 @@ +#requires -Version 5.1 +<# +.SYNOPSIS + 拉取 OceanPlugin(UE4.26 原生海洋 / 水下材质库) + +.DESCRIPTION + OceanPlugin 是第三方仓库(234 MB),体积大且非本项目代码,因此不入库。 + 本脚本把它下载到 Plugins/OceanPlugin。 + + 来源:https://github.com/UE4-OceanProject/OceanPlugin + 许可:允许在任何 UE4 项目中使用(含商业),无需署名; + 唯一限制是不能把插件本身当商品转售。 + 全文见拉取后的 LICENSE 文件。 + + 说明:本机 git 协议访问 GitHub 可能被重置,所以优先用 HTTPS 下载 zip; + 若下载失败,请按脚本末尾提示手动下载。 + +.PARAMETER Force + 已存在时也重新拉取。 + +.EXAMPLE + powershell -ExecutionPolicy Bypass -File Unreal\CarlaUE4\Plugins\setup_ocean_plugin.ps1 +#> + +param( + [switch]$Force +) + +$ErrorActionPreference = "Stop" + +# 脚本就在 Plugins/ 目录下,OceanPlugin 与它同级 +$target = Join-Path $PSScriptRoot "OceanPlugin" +$uplugin = Join-Path $target "OceanPlugin.uplugin" + +Write-Host "=== OceanPlugin 安装 ===" -ForegroundColor Cyan +Write-Host "目标目录: $target" + +if ((Test-Path $uplugin) -and (-not $Force)) { + Write-Host "[OK] 已安装,跳过。需要重装请加 -Force" -ForegroundColor Green + exit 0 +} + +if ((Test-Path $target) -and $Force) { + Write-Host "清理旧目录..." -ForegroundColor DarkGray + Remove-Item $target -Recurse -Force +} + +$zipUrl = "https://codeload.github.com/UE4-OceanProject/OceanPlugin/zip/refs/heads/master" +$tmpZip = Join-Path $env:TEMP "OceanPlugin.zip" +$tmpDir = Join-Path $env:TEMP "OceanPlugin_extract" + +$downloaded = $false +try { + Write-Host "下载中(约 200 MB)..." -ForegroundColor DarkGray + Invoke-WebRequest -Uri $zipUrl -OutFile $tmpZip -UseBasicParsing -TimeoutSec 1800 + $sizeMb = [math]::Round((Get-Item $tmpZip).Length / 1MB, 1) + Write-Host "[OK] 已下载 $sizeMb MB" -ForegroundColor Green + $downloaded = $true +} +catch { + Write-Host "[WARN] 自动下载失败:$($_.Exception.Message)" -ForegroundColor Yellow +} + +if ($downloaded) { + if (Test-Path $tmpDir) { Remove-Item $tmpDir -Recurse -Force } + Write-Host "解压..." -ForegroundColor DarkGray + Expand-Archive -Path $tmpZip -DestinationPath $tmpDir -Force + + # zip 里是一层 OceanPlugin-master/ 目录 + $inner = Get-ChildItem $tmpDir -Directory | Select-Object -First 1 + if (-not $inner) { + Write-Host "[ERROR] 解压结果为空" -ForegroundColor Red + exit 1 + } + + New-Item -ItemType Directory -Path $target -Force | Out-Null + Copy-Item (Join-Path $inner.FullName "*") $target -Recurse -Force + + # 去掉 git 历史,省 900 MB + $gitDir = Join-Path $target ".git" + if (Test-Path $gitDir) { Remove-Item $gitDir -Recurse -Force } + + Remove-Item $tmpZip -Force -ErrorAction SilentlyContinue + Remove-Item $tmpDir -Recurse -Force -ErrorAction SilentlyContinue +} + +# --- 校验 --- +Write-Host "" +if (Test-Path $uplugin) { + $files = Get-ChildItem $target -Recurse -File -ErrorAction SilentlyContinue + $mb = [math]::Round((($files | Measure-Object Length -Sum).Sum) / 1MB, 1) + Write-Host "[OK] OceanPlugin 安装完成:$($files.Count) 个文件,$mb MB" -ForegroundColor Green + + $keyAssets = @( + "Content\Ocean\Materials\M_Underwater_PP.uasset", + "Content\Ocean\Materials\M_Ocean_Versions\M_Ocean_Ultra.uasset", + "Content\Lake\Materials\M_Underwater_Lake_PP.uasset" + ) + foreach ($a in $keyAssets) { + $p = Join-Path $target $a + if (Test-Path $p) { + Write-Host "[OK] $(Split-Path $a -Leaf)" -ForegroundColor Green + } + else { + Write-Host "[WARN] 缺少关键资产: $a" -ForegroundColor Yellow + } + } + Write-Host "" + Write-Host "下一步:重新生成工程文件并编译" -ForegroundColor Cyan + exit 0 +} +else +{ + Write-Host "[ERROR] 安装失败" -ForegroundColor Red + Write-Host "" + Write-Host "请手动下载:" -ForegroundColor Yellow + Write-Host " 1. 打开 https://github.com/UE4-OceanProject/OceanPlugin" -ForegroundColor Yellow + Write-Host " 2. Code -> Download ZIP" -ForegroundColor Yellow + Write-Host " 3. 解压后把里面的内容放到:" -ForegroundColor Yellow + Write-Host " $target" -ForegroundColor Yellow + Write-Host " (该目录下应直接看到 OceanPlugin.uplugin)" -ForegroundColor Yellow + exit 1 +} diff --git a/Unreal/CarlaUE4/Source/CarlaUE4.Target.cs b/Unreal/CarlaUE4/Source/CarlaUE4.Target.cs index 6c4ce698c1..ec8f510f26 100644 --- a/Unreal/CarlaUE4/Source/CarlaUE4.Target.cs +++ b/Unreal/CarlaUE4/Source/CarlaUE4.Target.cs @@ -38,5 +38,10 @@ public CarlaUE4Target(TargetInfo Target) : base(Target) // URLab 插件的 MuJoCo 依赖需要 C++17(std::byte) CppStandard = CppStandardVersion.Cpp17; + + // 排查打包版崩溃时曾临时打开(Shipping 默认不写日志): + // 2026-11-08:打包版姿态与编辑器不一致,重新打开以便抓 tilt/mass/qfrc。 + // 打开后 Engine 模块需整体重编(约 700 个编译动作),出完包可以再关掉。 + bUseLoggingInShipping = true; } } diff --git a/Unreal/CarlaUE4/settings.json b/Unreal/CarlaUE4/settings.json index f071345afb..68130d3879 100644 --- a/Unreal/CarlaUE4/settings.json +++ b/Unreal/CarlaUE4/settings.json @@ -1,4 +1,4 @@ { "SettingsVersion": 1.2, "SimMode": "Multirotor" -} \ No newline at end of file +} diff --git a/Unreal/CarlaUE4/settings_drone.json b/Unreal/CarlaUE4/settings_drone.json new file mode 100644 index 0000000000..f071345afb --- /dev/null +++ b/Unreal/CarlaUE4/settings_drone.json @@ -0,0 +1,4 @@ +{ + "SettingsVersion": 1.2, + "SimMode": "Multirotor" +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/settings_rov.json b/Unreal/CarlaUE4/settings_rov.json new file mode 100644 index 0000000000..056c0a422c --- /dev/null +++ b/Unreal/CarlaUE4/settings_rov.json @@ -0,0 +1,117 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", + "SettingsVersion": 1.2, + "SimMode": "Rov", + "ClockSpeed": 1, + "PawnPaths": { + "DefaultQuadrotor": { + "PawnBP": "Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'" + }, + "DefaultRov": { + "PawnBP": "Class'/Script/AirSim.RovPawn'" + }, + "DefaultComputerVision": { + "PawnBP": "Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'" + } + }, + "ZBed": -30.0, + "EnableDepthClamp": true, + "BedStiffness": 2000.0, + "BedDamping": 300.0, + "BedContactMargin": 0.5, + "BedClampMargin": 0.3, + "EnableNeutralBuoyancy": true, + "EnableHydrodynamicDamping": true, + "OffZ": 0.05, + "Vehicles": { + "RovSimple": { + "VehicleType": "RovSimple", + "DefaultVehicleState": "Armed", + "PawnPath": "DefaultRov", + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + }, + "ZBed": -30.0, + "EnableDepthClamp": true, + "BedStiffness": 2000.0, + "BedDamping": 300.0, + "BedContactMargin": 0.5, + "BedClampMargin": 0.3, + "EnableNeutralBuoyancy": true, + "EnableHydrodynamicDamping": true, + "OffZ": 0.05, + "Cameras": { + "front_left_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.5, + "Y": -0.06, + "Z": 0.1, + "Pitch": 0.0, + "Roll": 0.0, + "Yaw": 0.0 + }, + "front_right_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.5, + "Y": 0.06, + "Z": 0.1, + "Pitch": 0.0, + "Roll": 0.0, + "Yaw": 0.0 + } + }, + "Sensors": { + "barometer": { + "SensorType": 1, + "Enabled": true + }, + "imu": { + "SensorType": 2, + "Enabled": true + }, + "magnetometer": { + "SensorType": 4, + "Enabled": true + }, + "downward_sonar": { + "SensorType": 5, + "Enabled": true, + "MinDistance": 0.1, + "MaxDistance": 50.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.15, + "Pitch": -90.0, + "Roll": 0.0, + "Yaw": 0.0 + } + }, + "X": 61.3, + "Y": -67.5, + "Z": 75.5, + "Pitch": 0.0, + "Roll": 0.0, + "Yaw": 0.0 + } + }, + "ViewMode": "SpringArmChase" +} \ No newline at end of file diff --git a/Util/BuildTools/Check.bat b/Util/BuildTools/Check.bat index 98d597f61b..d240fdf668 100644 --- a/Util/BuildTools/Check.bat +++ b/Util/BuildTools/Check.bat @@ -48,10 +48,12 @@ set VR_TESTS=false set PYTHON_API=false set RUN_BENCHMARK=false set AIR_TESTS=false +set WATER_TESTS=false set PYTHON_TESTS=false set MEASURE_TIME=true set UPLOAD_DOWNLOAD=false + rem set home_dir=%LOCAL_PATH%..\..\ rem 相对路径转换为完整的绝对路径 rem for %%i in ("%home_dir%") do set home_dir=%%~fi @@ -60,6 +62,20 @@ set python_path=%python_dir%python.exe set pip_path=%python_dir%Scripts\pip.exe echo python_path: %python_path% + +rem Get the directory where CarlaUE4 is located (refer to Package.bat). +for /f %%i in ('git rev-parse --short HEAD') do set CARLA_VERSION=%%i +if not defined CARLA_VERSION goto bad_exit + +rem If the "dirty" suffix is ​​present, it indicates that the package was compiled after switching between multiple versions, and this package is also used during testing. +rem (Solve the problem of not being able to find the executable file in the dirty directory during testing) +if exist %INSTALLATION_DIR%UE4Carla/%CARLA_VERSION%-dirty/ ( + set CARLA_VERSION=%CARLA_VERSION%-dirty +) + + + + :arg-parse if not "%1"=="" ( if "%1"=="--all" ( @@ -94,6 +110,11 @@ if not "%1"=="" ( shift ) + if "%1"=="--water" ( + set WATER_TESTS=true + shift + ) + if "%1"=="--vr" ( set VR_TESTS=true shift @@ -125,20 +146,28 @@ if not "%1"=="" ( goto :arg-parse ) +rem The directory of CarlaUE4.exe +set BUILD_FOLDER=%INSTALLATION_DIR%UE4Carla/%CARLA_VERSION%/ +rem debug only with rename (CARLA_VERSION to debug) +if %IS_DEBUG%==true ( + set BUILD_FOLDER=%INSTALLATION_DIR%UE4Carla\debug\ +) + +set exe_dir=%BUILD_FOLDER:\=/%WindowsNoEditor/% +set exe_path=%BUILD_FOLDER:/=\%WindowsNoEditor\CarlaUE4.exe + +if %WATER_TESTS%==true ( + echo switch to WATER configuration by replacing the settings.json file + if exist "%BUILD_FOLDER:/=\%WindowsNoEditor\settings_rov.json" ( + copy /Y "%BUILD_FOLDER:/=\%WindowsNoEditor\settings_rov.json" "%BUILD_FOLDER:/=\%WindowsNoEditor\settings.json" + ) +) + rem ============================================================================ rem -- Launch Serve for test --------------------------------------------------- rem ============================================================================ -rem Get the directory where CarlaUE4 is located (refer to Package.bat). -for /f %%i in ('git rev-parse --short HEAD') do set CARLA_VERSION=%%i -if not defined CARLA_VERSION goto bad_exit - -rem If the "dirty" suffix is ​​present, it indicates that the package was compiled after switching between multiple versions, and this package is also used during testing. -rem (Solve the problem of not being able to find the executable file in the dirty directory during testing) -if exist %INSTALLATION_DIR%UE4Carla/%CARLA_VERSION%-dirty/ ( - set CARLA_VERSION=%CARLA_VERSION%-dirty -) if %UPLOAD_DOWNLOAD%==true ( cd /d %ROOT_PATH%Util @@ -159,17 +188,6 @@ if %UPLOAD_DOWNLOAD%==true ( ) - -rem The directory of CarlaUE4.exe -set BUILD_FOLDER=%INSTALLATION_DIR%UE4Carla/%CARLA_VERSION%/ -rem debug only (rename with no dirty) -if %IS_DEBUG%==true ( - set BUILD_FOLDER=%INSTALLATION_DIR%UE4Carla\debug\ -) - -set exe_dir=%BUILD_FOLDER:\=/%WindowsNoEditor/% -set exe_path=%BUILD_FOLDER:/=\%WindowsNoEditor\CarlaUE4.exe - rem If exist CarlaUE4.exe process, kill it for /f "tokens=5" %%a in ('netstat -ano ^| findstr :3654') do taskkill /F /PID %%a :: use command "start" to launch a new service process. Otherwise stuck @@ -341,6 +359,38 @@ if %VR_TESTS%==true ( ) +rem ============================================================================ +rem -- Run water tests --------------------------------------------------------- +rem ============================================================================ + +call :get_current_time_in_seconds T_START_DO_TEST + + +if %WATER_TESTS%==true ( + echo Testing water... + echo Current directory: %cd% + for /l %%i in (14,-1,7) do ( + echo Running water tests for Python 3.%%i + call conda activate hutb_3.%%i + echo Current Python path: + where python + timeout /t 10 /nobreak > NUL + echo Switch to water mode... + python ./holoocean/airsim_rov.py --port 3654 + rem check test exit status + if %errorlevel% equ 0 ( + echo Water ROV test passed. + ) else ( + echo Water ROV test failed with errorlevel %errorlevel%. + goto bad_exit + ) + ) +) + + +rem ============================================================================ +rem -- Run Python packaging tests ---------------------------------------------- +rem ============================================================================ if %PYTHON_TESTS%==true ( echo Testing Python packaging... echo Current directory: %cd% @@ -356,10 +406,7 @@ if %PYTHON_TESTS%==true ( echo Python packaging test failed with level %errorlevel%. goto bad_exit ) -) - - -call :get_current_time_in_seconds T_END_DO_TEST +)call :get_current_time_in_seconds T_END_DO_TEST set /A ELAPSED_TIME=!T_END_DO_TEST! - !T_START_DO_TEST! if %MEASURE_TIME%==true if %VR_TESTS%==true echo %FILE_N% [TIME]: Running VR test took !ELAPSED_TIME! seconds. diff --git a/Util/BuildTools/Package.bat b/Util/BuildTools/Package.bat index bc8730ddf6..9bbad9db7d 100644 --- a/Util/BuildTools/Package.bat +++ b/Util/BuildTools/Package.bat @@ -170,12 +170,14 @@ if %DO_PACKAGE%==true ( -WaitMutex^ -FromMsBuild^ "%ROOT_PATH%Unreal/CarlaUE4/CarlaUE4.uproject" + rem 限制并发编译进程数,避免低内存机器编译引擎时出现 C1076/C3859(编译器 OOM) call "%UE4_ROOT%\Engine\Build\BatchFiles\Build.bat"^ CarlaUE4Editor^ Win64^ Development^ -WaitMutex^ -FromMsBuild^ + -MaxParallelActions=4^ "%ROOT_PATH%Unreal/CarlaUE4/CarlaUE4.uproject" if errorlevel 1 goto error_build_editor @@ -194,6 +196,7 @@ if %DO_PACKAGE%==true ( %PACKAGE_CONFIG%^ -WaitMutex^ -FromMsBuild^ + -MaxParallelActions=4^ "%ROOT_PATH%Unreal/CarlaUE4/CarlaUE4.uproject" if errorlevel 1 goto error_build @@ -289,16 +292,20 @@ if %DO_COPY_FILES%==true ( echo f | xcopy /y "!XCOPY_FROM!Unreal\CarlaUE4\Content\Carla\HDMaps\*.pcd" "!XCOPY_TO!HDMaps\" echo f | xcopy /y "!XCOPY_FROM!Unreal\CarlaUE4\Content\Carla\HDMaps\Readme.md" "!XCOPY_TO!HDMaps\README" :: 无人机配置文件 - echo f | xcopy /y "!XCOPY_FROM!Build\AirSim\Unreal\Environments\Blocks\settings.json" "!XCOPY_TO!settings.json" - if exist "!XCOPY_FROM!Plugins" ( - echo d | xcopy /y /s "!XCOPY_FROM!Plugins" "!XCOPY_TO!Plugins" - ) + echo f | xcopy /y "!XCOPY_FROM!Unreal\CarlaUE4\settings.json" "!XCOPY_TO!settings.json" + echo f | xcopy /y "!XCOPY_FROM!Unreal\CarlaUE4\settings_drone.json" "!XCOPY_TO!settings_drone.json" + :: 水下机器人载具配置文件 + echo f | xcopy /y "!XCOPY_FROM!Unreal\CarlaUE4\settings_rov.json" "!XCOPY_TO!settings_rov.json" + :: AirSim 联合仿真示例与测试套件 + echo d | xcopy /y /s "!XCOPY_FROM!PythonAPI\examples\air" "!XCOPY_TO!PythonAPI\examples\air" :: 下载 Mujoco - set Mujoco_FILE_NAME=mujoco-3.3.5-windows-x86_64.zip + :: 版本以 third_party/install/MuJoCo/include/mujoco/mujoco.h 的 mjVERSION_HEADER 为准, + :: 当前为 3008001(= 3.8.1)。历史上这里写的是 3.3.5,和实际链接的 DLL 不一致,已统一。 + set Mujoco_FILE_NAME=mujoco-3.8.1-windows-x86_64.zip :: !! 为延迟变量,执行时才扩展,需要 setlocal enabledelayedexpansion set Mujoco_TEMP_FILE_DIR="!XCOPY_FROM!Build\dependencies\Plugins\!Mujoco_FILE_NAME!" - set Mujoco_REPO=https://github.com/google-deepmind/mujoco/releases/download/3.3.5/%Mujoco_FILE_NAME% + set Mujoco_REPO=https://github.com/google-deepmind/mujoco/releases/download/3.8.1/%Mujoco_FILE_NAME% if exist "!Mujoco_TEMP_FILE_DIR!" ( echo "%ProgramW6432%/7-Zip/7z.exe" x "!Mujoco_TEMP_FILE_DIR!" -o"!XCOPY_FROM!Build\dependencies\Plugins\mujoco" -y diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk index 4ebae6b04f..dd3bfc5282 100644 --- a/Util/BuildTools/Windows.mk +++ b/Util/BuildTools/Windows.mk @@ -94,6 +94,9 @@ check.air: check.vr: @"${CARLA_BUILD_TOOLS_FOLDER}/Check.bat" --vr $(ARGS) +check.water: + @"${CARLA_BUILD_TOOLS_FOLDER}/Check.bat" --water $(ARGS) + check.python: @"${CARLA_BUILD_TOOLS_FOLDER}/Check.bat" --python $(ARGS) diff --git a/Util/BuildTools/enable_carsim_to_uproject.py b/Util/BuildTools/enable_carsim_to_uproject.py index d3f5d5c1db..24bf31c7b1 100644 --- a/Util/BuildTools/enable_carsim_to_uproject.py +++ b/Util/BuildTools/enable_carsim_to_uproject.py @@ -29,7 +29,7 @@ def main(): args = argparser.parse_args() # Read uproject (json) file - uproject_file = open(args.file, 'r') + uproject_file = open(args.file, 'r', encoding='utf-8-sig') uproject_json = json.load(uproject_file) uproject_file.close() @@ -56,7 +56,7 @@ def main(): # Save file if there are changes to do if should_do_changes: - uproject_file = open(args.file, 'w') + uproject_file = open(args.file, 'w', encoding='utf-8') uproject_file.write(json.dumps(uproject_json, indent = 4, sort_keys=True)) uproject_file.close() diff --git a/Util/BuildTools/enable_simready_to_uproject.py b/Util/BuildTools/enable_simready_to_uproject.py index fe7905f1ab..2f5971aac4 100644 --- a/Util/BuildTools/enable_simready_to_uproject.py +++ b/Util/BuildTools/enable_simready_to_uproject.py @@ -35,7 +35,7 @@ def main(): args = argparser.parse_args() # Read uproject (json) file - uproject_file = open(args.file, 'r') + uproject_file = open(args.file, 'r', encoding='utf-8-sig') uproject_json = json.load(uproject_file) uproject_file.close() @@ -65,7 +65,7 @@ def main(): # Save file if there are changes to do if should_do_changes: - uproject_file = open(args.file, 'w') + uproject_file = open(args.file, 'w', encoding='utf-8') uproject_file.write(json.dumps(uproject_json, indent = 4)) uproject_file.close() diff --git a/setup.bat b/setup.bat index bf4880d81d..fbbaf0f24c 100644 --- a/setup.bat +++ b/setup.bat @@ -1,4 +1,4 @@ -@echo off +@echo off :: resolve display issue in Chinese (add support for utf-8) chcp 65001 > nul setlocal EnableDelayedExpansion @@ -182,6 +182,15 @@ if exist "%cd%\Build\dependencies\" ( ) else ( echo RoadRunner Plugins already exists. ) + rem Install OceanPlugin (third-party, downloaded on demand, not stored in repo) + if not exist "%cd%\Unreal\CarlaUE4\Plugins\OceanPlugin\OceanPlugin.uplugin" ( + echo Installing OceanPlugin ... + powershell -NoProfile -ExecutionPolicy Bypass -File "%cd%\Unreal\CarlaUE4\Plugins\setup_ocean_plugin.ps1" + if errorlevel 1 exit /b 1 + ) else ( + echo OceanPlugin already exists. + ) + rem Unzip CesiumForUnreal Plugin if not exist "%cd%\Unreal\CarlaUE4\Plugins\CesiumForUnreal" ( echo Unzipping CesiumForUnreal Plugin ... @@ -206,8 +215,22 @@ if exist "%cd%\Build\dependencies\" ( set UnrealRoboticsLab_FOLDER=%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\ echo UnrealRoboticsLab_FOLDER: %UnrealRoboticsLab_FOLDER% echo "%UnrealRoboticsLab_FOLDER%third_party\install" folder... + rem MuJoCo 版本以 third_party/install/MuJoCo/include/mujoco/mujoco.h 的 + rem mjVERSION_HEADER 为准:当前 3008001 = 3.8.1(历史压缩包名还叫 3.3.5,内容其实是 3.8.1)。 + rem 这里按 3.8.1 -> 历史命名 3.3.5 -> 在线下载 的顺序找。 + set "MUJOCO_ZIP=%cd%\Build\dependencies\Plugins\mujoco-3.8.1-windows-x86_64.zip" + if not exist "!MUJOCO_ZIP!" set "MUJOCO_ZIP=%cd%\Build\dependencies\Plugins\mujoco-3.3.5-windows-x86_64.zip" + if not exist "!MUJOCO_ZIP!" set "MUJOCO_ZIP=%cd%\Plugins\mujoco-3.8.1-windows-x86_64.zip" + if not exist "!MUJOCO_ZIP!" set "MUJOCO_ZIP=%cd%\Plugins\mujoco-3.3.5-windows-x86_64.zip" + if not exist "!MUJOCO_ZIP!" ( + echo %FILE_N% Downloading MuJoCo 3.8.1 ... + if not exist "%cd%\Build\dependencies\Plugins" mkdir "%cd%\Build\dependencies\Plugins" + powershell -NoProfile -ExecutionPolicy Bypass -Command "Invoke-WebRequest -Uri 'https://github.com/google-deepmind/mujoco/releases/download/3.8.1/mujoco-3.8.1-windows-x86_64.zip' -OutFile '%cd%\Build\dependencies\Plugins\mujoco-3.8.1-windows-x86_64.zip'" + set "MUJOCO_ZIP=%cd%\Build\dependencies\Plugins\mujoco-3.8.1-windows-x86_64.zip" + ) + echo MuJoCo archive: !MUJOCO_ZIP! if not exist "%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\third_party\install" ( - "prerequisites\7zip\7z.exe" x "Plugins\mujoco-3.3.5-windows-x86_64.zip" -o"%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\third_party\install\MuJoCo" -y + "prerequisites\7zip\7z.exe" x "!MUJOCO_ZIP!" -o"%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\third_party\install\MuJoCo" -y "prerequisites\7zip\7z.exe" x "Plugins\CoACD.zip" -o"%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\third_party\install\" -y "prerequisites\7zip\7z.exe" x "Plugins\libzmq.zip" -o"%cd%\Unreal\CarlaUE4\Plugins\UnrealRoboticsLab\third_party\install\" -y ) else ( diff --git a/setup.sh b/setup.sh index 0914971fdb..365361105a 100755 --- a/setup.sh +++ b/setup.sh @@ -359,6 +359,26 @@ if [ -d "$DEPENDENCIES_DIR" ]; then log "glTFForUE4 Plugin already exists." fi + # --- OceanPlugin(第三方 234 MB,不入库,按需下载)--- + # CarlaUE4.uproject 以 Enabled 引用了该插件,缺失时 UBT 会直接报 + # "Unable to find plugin 'OceanPlugin'" 并中断 CarlaUE4Editor 构建。 + if [ ! -f "$PLUGINS_DIR/OceanPlugin/OceanPlugin.uplugin" ]; then + log "Downloading OceanPlugin ..." + OCEAN_TMP=$(mktemp -d) + if curl -L --retry 3 --connect-timeout 30 -o "$OCEAN_TMP/ocean.zip" \ + "https://codeload.github.com/UE4-OceanProject/OceanPlugin/zip/refs/heads/master" \ + && unzip -q "$OCEAN_TMP/ocean.zip" -d "$OCEAN_TMP/x"; then + mkdir -p "$PLUGINS_DIR/OceanPlugin" + cp -a "$OCEAN_TMP"/x/OceanPlugin-master/. "$PLUGINS_DIR/OceanPlugin/" + log "OceanPlugin installed." + else + warn " OceanPlugin 下载失败,CarlaUE4Editor 构建会因缺少插件而失败" + fi + rm -rf "$OCEAN_TMP" + else + log "OceanPlugin already exists." + fi + # --- UnrealRoboticsLab 第三方依赖 --- log "Initial UnrealRoboticsLab dependencies..." if [ ! -d "$URLAB_THIRD_PARTY" ]; then