CARLA
 
载入中...
搜索中...
未找到
ChronoMovementComponent.cpp
浏览该文件的文档.
1// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
2// de Barcelona (UAB).
3// Copyright (c) 2019 Intel Corporation
4//
5// This work is licensed under the terms of the MIT license.
6// For a copy, see <https://opensource.org/licenses/MIT>.
7
11
13#include <carla/rpc/String.h>
14#ifdef WITH_CHRONO
15#include "chrono_vehicle/utils/ChUtilsJSON.h"
16#endif
19
20
23 uint64_t MaxSubsteps,
24 float MaxSubstepDeltaTime,
25 FString VehicleJSON,
26 FString PowertrainJSON,
27 FString TireJSON,
28 FString BaseJSONPath)
29{
30 #ifdef WITH_CHRONO
31 UChronoMovementComponent* ChronoMovementComponent = NewObject<UChronoMovementComponent>(Vehicle);
32 if (!VehicleJSON.IsEmpty())
33 {
34 ChronoMovementComponent->VehicleJSON = VehicleJSON;
35 }
36 if (!PowertrainJSON.IsEmpty())
37 {
38 ChronoMovementComponent->PowertrainJSON = PowertrainJSON;
39 }
40 if (!TireJSON.IsEmpty())
41 {
42 ChronoMovementComponent->TireJSON = TireJSON;
43 }
44 if (!BaseJSONPath.IsEmpty())
45 {
46 ChronoMovementComponent->BaseJSONPath = BaseJSONPath;
47 }
48 ChronoMovementComponent->MaxSubsteps = MaxSubsteps;
49 ChronoMovementComponent->MaxSubstepDeltaTime = MaxSubstepDeltaTime;
50 Vehicle->SetCarlaMovementComponent(ChronoMovementComponent);
51 ChronoMovementComponent->RegisterComponent();
52 #else
53 UE_LOG(LogCarla, Warning, TEXT("Error: Chrono is not enabled") );
54 #endif
55}
56
57#ifdef WITH_CHRONO
58
59using namespace chrono;
60using namespace chrono::vehicle;
61
62constexpr double CMTOM = 0.01;
63ChVector<> UE4LocationToChrono(const FVector& Location)
64{
65 return CMTOM*ChVector<>(Location.X, -Location.Y, Location.Z);
66}
67constexpr double MTOCM = 100;
68FVector ChronoToUE4Location(const ChVector<>& position)
69{
70 return MTOCM*FVector(position.x(), -position.y(), position.z());
71}
72ChVector<> UE4DirectionToChrono(const FVector& Location)
73{
74 return ChVector<>(Location.X, -Location.Y, Location.Z);
75}
76FVector ChronoToUE4Direction(const ChVector<>& position)
77{
78 return FVector(position.x(), -position.y(), position.z());
79}
80ChQuaternion<> UE4QuatToChrono(const FQuat& Quat)
81{
82 return ChQuaternion<>(Quat.W, -Quat.X, Quat.Y, -Quat.Z);
83}
84FQuat ChronoToUE4Quat(const ChQuaternion<>& quat)
85{
86 return FQuat(-quat.e1(), quat.e2(), -quat.e3(), quat.e0());
87}
88
89UERayCastTerrain::UERayCastTerrain(
90 ACarlaWheeledVehicle* UEVehicle,
91 chrono::vehicle::ChVehicle* ChrVehicle)
92 : CarlaVehicle(UEVehicle), ChronoVehicle(ChrVehicle) {}
93
94std::pair<bool, FHitResult>
95 UERayCastTerrain::GetTerrainProperties(const FVector &Location) const
96{
97 const double MaxDistance = 1000000;
98 FVector StartLocation = Location;
99 FVector EndLocation = Location + FVector(0,0,-1)*MaxDistance; // 向下搜索
100 FHitResult Hit;
101 FCollisionQueryParams CollisionQueryParams;
102 CollisionQueryParams.AddIgnoredActor(CarlaVehicle);
103 bool bDidHit = CarlaVehicle->GetWorld()->LineTraceSingleByChannel(
104 Hit,
105 StartLocation,
106 EndLocation,
107 ECC_GameTraceChannel2, // 摄像机(任意碰撞)
108 CollisionQueryParams,
109 FCollisionResponseParams()
110 );
111 return std::make_pair(bDidHit, Hit);
112}
113
114double UERayCastTerrain::GetHeight(const ChVector<>& loc) const
115{
116 FVector Location = ChronoToUE4Location(loc + ChVector<>(0,0,0.5)); // 一个小偏移量,用于正确检测地面
117 auto point_pair = GetTerrainProperties(Location);
118 if (point_pair.first)
119 {
120 double Height = CMTOM*static_cast<double>(point_pair.second.Location.Z);
121 return Height;
122 }
123 return -1000000.0;
124}
125ChVector<> UERayCastTerrain::GetNormal(const ChVector<>& loc) const
126{
127 FVector Location = ChronoToUE4Location(loc);
128 auto point_pair = GetTerrainProperties(Location);
129 if (point_pair.first)
130 {
131 FVector Normal = point_pair.second.Normal;
132 auto ChronoNormal = UE4DirectionToChrono(Normal);
133 return ChronoNormal;
134 }
135 return UE4DirectionToChrono(FVector(0,0,1));
136}
137float UERayCastTerrain::GetCoefficientFriction(const ChVector<>& loc) const
138{
139 return 1;
140}
141
143{
144 Super::BeginPlay();
145
147
148 // // // Chrono System
149 Sys.Set_G_acc(ChVector<>(0, 0, -9.81));
150 Sys.SetSolverType(ChSolver::Type::BARZILAIBORWEIN);
151 Sys.SetSolverMaxIterations(150);
152 Sys.SetMaxPenetrationRecoverySpeed(4.0);
153
154 InitializeChronoVehicle();
155
156 // 创建地形
157 Terrain = chrono_types::make_shared<UERayCastTerrain>(CarlaVehicle, Vehicle.get());
158
159 CarlaVehicle->OnActorHit.AddDynamic(
161 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.AddDynamic(
163 CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(
164 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Overlap);
165}
166
167void UChronoMovementComponent::InitializeChronoVehicle()
168{
169 // 初始位置设有小偏移量,以防穿透地面
170 FVector VehicleLocation = CarlaVehicle->GetActorLocation() + FVector(0,0,25);
171 FQuat VehicleRotation = CarlaVehicle->GetActorRotation().Quaternion();
172 auto ChronoLocation = UE4LocationToChrono(VehicleLocation);
173 auto ChronoRotation = UE4QuatToChrono(VehicleRotation);
174
175 // 为车辆JSON文件设置基本路径
176 vehicle::SetDataPath(carla::rpc::FromFString(BaseJSONPath));
177
178 std::string BasePath_string = carla::rpc::FromFString(BaseJSONPath);
179
180 // 为json文件创建完整路径
181 //不要使用vehicle::GetDataFile(),因为chrono库中的字符串会与unreal的std库发生冲突
182 std::string VehicleJSON_string = carla::rpc::FromFString(VehicleJSON);
183 std::string VehiclePath_string = BasePath_string + VehicleJSON_string;
184 FString VehicleJSONPath = carla::rpc::ToFString(VehiclePath_string);
185
186 std::string PowerTrainJSON_string = carla::rpc::FromFString(PowertrainJSON);
187 std::string PowerTrain_string = BasePath_string + PowerTrainJSON_string;
188 FString PowerTrainJSONPath = carla::rpc::ToFString(PowerTrain_string);
189
190 std::string TireJSON_string = carla::rpc::FromFString(TireJSON);
191 std::string Tire_string = BasePath_string + TireJSON_string;
192 FString TireJSONPath = carla::rpc::ToFString(Tire_string);
193
194 UE_LOG(LogCarla, Log, TEXT("Loading Chrono files: Vehicle: %s, PowerTrain: %s, Tire: %s"),
195 *VehicleJSONPath,
196 *PowerTrainJSONPath,
197 *TireJSONPath);
198 // 创建JSON车辆文件
199 Vehicle = chrono_types::make_shared<WheeledVehicle>(
200 &Sys,
201 VehiclePath_string);
202 Vehicle->Initialize(ChCoordsys<>(ChronoLocation, ChronoRotation));
203 Vehicle->GetChassis()->SetFixed(false);
204 // 创建并初始化传动系统
205 auto powertrain = ReadPowertrainJSON(
206 PowerTrain_string);
207 Vehicle->InitializePowertrain(powertrain);
208 // 创建并初始化轮胎
209 for (auto& axle : Vehicle->GetAxles()) {
210 for (auto& wheel : axle->GetWheels()) {
211 auto tire = ReadTireJSON(Tire_string);
212 Vehicle->InitializeTire(tire, wheel, VisualizationType::MESH);
213 }
214 }
215}
216
218{
220 auto PowerTrain = Vehicle->GetPowertrain();
221 if (PowerTrain)
222 {
224 {
225 PowerTrain->SetDriveMode(ChPowertrain::DriveMode::REVERSE);
226 }
227 else
228 {
229 PowerTrain->SetDriveMode(ChPowertrain::DriveMode::FORWARD);
230 }
231 }
232}
233
234void UChronoMovementComponent::TickComponent(float DeltaTime,
235 ELevelTick TickType,
236 FActorComponentTickFunction* ThisTickFunction)
237{
238 TRACE_CPUPROFILER_EVENT_SCOPE(UChronoMovementComponent::TickComponent);
239 if (DeltaTime > MaxSubstepDeltaTime)
240 {
241 uint64_t NumberSubSteps =
242 FGenericPlatformMath::FloorToInt(DeltaTime/MaxSubstepDeltaTime);
243 if (NumberSubSteps < MaxSubsteps)
244 {
245 for (uint64_t i = 0; i < NumberSubSteps; ++i)
246 {
247 AdvanceChronoSimulation(MaxSubstepDeltaTime);
248 }
249 float RemainingTime = DeltaTime - NumberSubSteps*MaxSubstepDeltaTime;
250 if (RemainingTime > 0)
251 {
252 AdvanceChronoSimulation(RemainingTime);
253 }
254 }
255 else
256 {
257 double SubDelta = DeltaTime / MaxSubsteps;
258 for (uint64_t i = 0; i < MaxSubsteps; ++i)
259 {
260 AdvanceChronoSimulation(SubDelta);
261 }
262 }
263 }
264 else
265 {
266 AdvanceChronoSimulation(DeltaTime);
267 }
268
269 const auto ChronoPositionOffset = ChVector<>(0,0,-0.25f);
270 auto VehiclePos = Vehicle->GetVehiclePos() + ChronoPositionOffset;
271 auto VehicleRot = Vehicle->GetVehicleRot();
272 double Time = Vehicle->GetSystem()->GetChTime();
273
274 FVector NewLocation = ChronoToUE4Location(VehiclePos);
275 FQuat NewRotation = ChronoToUE4Quat(VehicleRot);
276 if(NewLocation.ContainsNaN() || NewRotation.ContainsNaN())
277 {
278 UE_LOG(LogCarla, Warning, TEXT(
279 "Error: Chrono vehicle position or rotation contains NaN. Disabling chrono physics..."));
281 return;
282 }
283 CarlaVehicle->SetActorLocation(NewLocation);
284 FRotator NewRotator = NewRotation.Rotator();
285 // 添加小幅旋转以补偿chrono偏移量
286 const float ChronoPitchOffset = 2.5f;
287 NewRotator.Add(ChronoPitchOffset, 0.f, 0.f);
288 CarlaVehicle->SetActorRotation(NewRotator);
289}
290
291void UChronoMovementComponent::AdvanceChronoSimulation(float StepSize)
292{
293 double Time = Vehicle->GetSystem()->GetChTime();
294 double Throttle = VehicleControl.Throttle;
295 double Steering = -VehicleControl.Steer; // 右舵改为左舵
297 Vehicle->Synchronize(Time, {Steering, Throttle, Brake}, *Terrain.get());
298 Vehicle->Advance(StepSize);
299 Sys.DoStepDynamics(StepSize);
300}
301
303{
304 if (Vehicle)
305 {
306 return ChronoToUE4Location(
307 Vehicle->GetVehiclePointVelocity(ChVector<>(0,0,0)));
308 }
309 return FVector();
310}
311
313{
314 if (Vehicle)
315 {
316 auto PowerTrain = Vehicle->GetPowertrain();
317 if (PowerTrain)
318 {
319 return PowerTrain->GetCurrentTransmissionGear();
320 }
321 }
322 return 0;
323}
324
326{
327 if (Vehicle)
328 {
329 return GetVelocity().X;
330 }
331 return 0.f;
332}
333
334void UChronoMovementComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
335{
336 if(!CarlaVehicle)
337 {
338 return;
339 }
340 // 重置回调函数以响应碰撞
341 CarlaVehicle->OnActorHit.RemoveDynamic(
343 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.RemoveDynamic(
345 CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(
346 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
347}
348#endif
349
354
356{
357 this->SetComponentTickEnabled(false);
359 CarlaVehicle->OnActorHit.RemoveDynamic(this, &UChronoMovementComponent::OnVehicleHit);
360 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.RemoveDynamic(
362 CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(
363 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
365}
366
368 AActor *OtherActor,
369 FVector NormalImpulse,
370 const FHitResult &Hit)
371{
372 carla::log_warning("Chrono physics does not support collisions yet, reverting to default PhysX physics.");
374}
375
376// 在车辆网格重叠时,仅当启用了carsim时才有效
377// (此事件在与静态环境重叠时触发)
379 UPrimitiveComponent* OverlappedComponent,
380 AActor* OtherActor,
381 UPrimitiveComponent* OtherComp,
382 int32 OtherBodyIndex,
383 bool bFromSweep,
384 const FHitResult & SweepResult)
385{
386 if (OtherComp->GetCollisionResponseToChannel(
387 ECollisionChannel::ECC_WorldDynamic) ==
388 ECollisionResponse::ECR_Block)
389 {
390 carla::log_warning("Chrono physics does not support collisions yet, reverting to default PhysX physics.");
392 }
393}
FVehicleControl Control
Definition ActorData.h:119
UE_LOG(LogCarla, Log, TEXT("UActorDispatcher::Destroying actor: '%s' %x"), *Id, Actor)
TSharedPtr< const FActorInfo > carla::rpc::ActorState UWorld Actor
Base class for CARLA wheeled vehicles.
virtual void ProcessControl(FVehicleControl &Control)
void EnableUE4VehiclePhysics(bool bResetVelocity=true)
void OnVehicleHit(AActor *Actor, AActor *OtherActor, FVector NormalImpulse, const FHitResult &Hit)
static void CreateChronoMovementComponent(ACarlaWheeledVehicle *Vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime, FString VehicleJSON="", FString PowertrainJSON="", FString TireJSON="", FString BaseJSONPath="")
void OnVehicleOverlap(UPrimitiveComponent *OverlappedComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex, bool bFromSweep, const FHitResult &SweepResult)
virtual void DisableSpecialPhysics() override
static void CreateDefaultMovementComponent(ACarlaWheeledVehicle *Vehicle)
geom::Location Location
static void log_warning(Args &&... args)
Definition Logging.h:101