15#include "chrono_vehicle/utils/ChUtilsJSON.h"
24 float MaxSubstepDeltaTime,
26 FString PowertrainJSON,
50 Vehicle->SetCarlaMovementComponent(ChronoMovementComponent);
51 ChronoMovementComponent->RegisterComponent();
53 UE_LOG(LogCarla, Warning, TEXT(
"Error: Chrono is not enabled") );
59using namespace chrono;
60using namespace chrono::vehicle;
62constexpr double CMTOM = 0.01;
63ChVector<> UE4LocationToChrono(
const FVector& Location)
65 return CMTOM*ChVector<>(Location.X, -Location.Y, Location.Z);
67constexpr double MTOCM = 100;
68FVector ChronoToUE4Location(
const ChVector<>& position)
70 return MTOCM*FVector(position.x(), -position.y(), position.z());
72ChVector<> UE4DirectionToChrono(
const FVector& Location)
76FVector ChronoToUE4Direction(
const ChVector<>& position)
78 return FVector(position.x(), -position.y(), position.z());
80ChQuaternion<> UE4QuatToChrono(
const FQuat& Quat)
82 return ChQuaternion<>(Quat.W, -Quat.X, Quat.Y, -Quat.Z);
84FQuat ChronoToUE4Quat(
const ChQuaternion<>& quat)
86 return FQuat(-quat.e1(), quat.e2(), -quat.e3(), quat.e0());
89UERayCastTerrain::UERayCastTerrain(
91 chrono::vehicle::ChVehicle* ChrVehicle)
92 : CarlaVehicle(UEVehicle), ChronoVehicle(ChrVehicle) {}
94std::pair<bool, FHitResult>
95 UERayCastTerrain::GetTerrainProperties(
const FVector &Location)
const
97 const double MaxDistance = 1000000;
99 FVector EndLocation =
Location + FVector(0,0,-1)*MaxDistance;
101 FCollisionQueryParams CollisionQueryParams;
102 CollisionQueryParams.AddIgnoredActor(CarlaVehicle);
103 bool bDidHit = CarlaVehicle->GetWorld()->LineTraceSingleByChannel(
107 ECC_GameTraceChannel2,
108 CollisionQueryParams,
109 FCollisionResponseParams()
111 return std::make_pair(bDidHit, Hit);
114double UERayCastTerrain::GetHeight(
const ChVector<>& loc)
const
116 FVector
Location = ChronoToUE4Location(loc + ChVector<>(0,0,0.5));
117 auto point_pair = GetTerrainProperties(Location);
118 if (point_pair.first)
120 double Height = CMTOM*
static_cast<double>(point_pair.second.Location.Z);
125ChVector<> UERayCastTerrain::GetNormal(
const ChVector<>& loc)
const
127 FVector
Location = ChronoToUE4Location(loc);
128 auto point_pair = GetTerrainProperties(Location);
129 if (point_pair.first)
131 FVector Normal = point_pair.second.Normal;
132 auto ChronoNormal = UE4DirectionToChrono(Normal);
135 return UE4DirectionToChrono(FVector(0,0,1));
137float UERayCastTerrain::GetCoefficientFriction(
const ChVector<>& loc)
const
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);
154 InitializeChronoVehicle();
161 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.AddDynamic(
164 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Overlap);
167void UChronoMovementComponent::InitializeChronoVehicle()
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);
176 vehicle::SetDataPath(carla::rpc::FromFString(
BaseJSONPath));
178 std::string BasePath_string = carla::rpc::FromFString(
BaseJSONPath);
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);
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);
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);
194 UE_LOG(LogCarla, Log, TEXT(
"Loading Chrono files: Vehicle: %s, PowerTrain: %s, Tire: %s"),
199 Vehicle = chrono_types::make_shared<WheeledVehicle>(
202 Vehicle->Initialize(ChCoordsys<>(ChronoLocation, ChronoRotation));
203 Vehicle->GetChassis()->SetFixed(
false);
205 auto powertrain = ReadPowertrainJSON(
207 Vehicle->InitializePowertrain(powertrain);
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);
220 auto PowerTrain =
Vehicle->GetPowertrain();
225 PowerTrain->SetDriveMode(ChPowertrain::DriveMode::REVERSE);
229 PowerTrain->SetDriveMode(ChPowertrain::DriveMode::FORWARD);
234void UChronoMovementComponent::TickComponent(
float DeltaTime,
236 FActorComponentTickFunction* ThisTickFunction)
238 TRACE_CPUPROFILER_EVENT_SCOPE(UChronoMovementComponent::TickComponent);
241 uint64_t NumberSubSteps =
245 for (uint64_t i = 0; i < NumberSubSteps; ++i)
250 if (RemainingTime > 0)
252 AdvanceChronoSimulation(RemainingTime);
260 AdvanceChronoSimulation(SubDelta);
266 AdvanceChronoSimulation(DeltaTime);
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();
274 FVector NewLocation = ChronoToUE4Location(VehiclePos);
275 FQuat NewRotation = ChronoToUE4Quat(VehicleRot);
276 if(NewLocation.ContainsNaN() || NewRotation.ContainsNaN())
278 UE_LOG(LogCarla, Warning, TEXT(
279 "Error: Chrono vehicle position or rotation contains NaN. Disabling chrono physics..."));
284 FRotator NewRotator = NewRotation.Rotator();
286 const float ChronoPitchOffset = 2.5f;
287 NewRotator.Add(ChronoPitchOffset, 0.f, 0.f);
291void UChronoMovementComponent::AdvanceChronoSimulation(
float StepSize)
293 double Time =
Vehicle->GetSystem()->GetChTime();
297 Vehicle->Synchronize(Time, {Steering, Throttle, Brake}, *
Terrain.get());
299 Sys.DoStepDynamics(StepSize);
306 return ChronoToUE4Location(
307 Vehicle->GetVehiclePointVelocity(ChVector<>(0,0,0)));
316 auto PowerTrain =
Vehicle->GetPowertrain();
319 return PowerTrain->GetCurrentTransmissionGear();
334void UChronoMovementComponent::EndPlay(
const EEndPlayReason::Type EndPlayReason)
343 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.RemoveDynamic(
346 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
357 this->SetComponentTickEnabled(
false);
360 CarlaVehicle->GetMesh()->OnComponentBeginOverlap.RemoveDynamic(
363 ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
369 FVector NormalImpulse,
370 const FHitResult &Hit)
372 carla::log_warning(
"Chrono physics does not support collisions yet, reverting to default PhysX physics.");
382 int32 OtherBodyIndex,
384 const FHitResult & SweepResult)
386 if (OtherComp->GetCollisionResponseToChannel(
387 ECollisionChannel::ECC_WorldDynamic) ==
388 ECollisionResponse::ECR_Block)
390 carla::log_warning(
"Chrono physics does not support collisions yet, reverting to default PhysX physics.");
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)
virtual void BeginPlay() override
virtual int32 GetVehicleCurrentGear() const
virtual float GetVehicleForwardSpeed() const
void EnableUE4VehiclePhysics(bool bResetVelocity=true)
virtual FVector GetVelocity() const
ACarlaWheeledVehicle * CarlaVehicle
void DisableUE4VehiclePhysics()
float MaxSubstepDeltaTime
void OnVehicleHit(AActor *Actor, AActor *OtherActor, FVector NormalImpulse, const FHitResult &Hit)
FVehicleControl VehicleControl
static void CreateChronoMovementComponent(ACarlaWheeledVehicle *Vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime, FString VehicleJSON="", FString PowertrainJSON="", FString TireJSON="", FString BaseJSONPath="")
void DisableChronoPhysics()
void OnVehicleOverlap(UPrimitiveComponent *OverlappedComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex, bool bFromSweep, const FHitResult &SweepResult)
virtual void DisableSpecialPhysics() override
static void CreateDefaultMovementComponent(ACarlaWheeledVehicle *Vehicle)
static void log_warning(Args &&... args)