CarSimManagerComponent.cpp

浏览该文件的文档.

// Copyright (c) 2021 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
// Copyright (c) 2019 Intel Corporation
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "CarSimManagerComponent.h"
#include "Carla/Game/CarlaEngine.h"
#include "Carla/Vehicle/CarlaWheeledVehicle.h"
#include "Carla/Util/EmptyActor.h"
 
#ifdef WITH_CARSIM
#include "CarSimMovementComponent.h"
#include "VsVar.h"
#endif
 
void UCarSimManagerComponent::CreateCarsimComponent(
      ACarlaWheeledVehicle* Vehicle, FString Simfile)
{
  #ifdef WITH_CARSIM
  UCarSimManagerComponent* CarSimManagerComponent = NewObject<UCarSimManagerComponent>(Vehicle);
  CarSimManagerComponent->SimfilePath = Simfile;
  Vehicle->SetCarlaMovementComponent(CarSimManagerComponent);
  CarSimManagerComponent->RegisterComponent();
  #else
  UE_LOG(LogCarla, Warning, TEXT("Error: CarSim plugin is not enabled") );
  #endif
}
 
void UCarSimManagerComponent::BeginPlay()
{
  Super::BeginPlay();
  #ifdef WITH_CARSIM
 
  if(!CarlaVehicle)
  {
    UE_LOG(LogCarla, Warning, TEXT("Error: Owner is not properly set for UCarSimManagerComponent") );
    return;
  }
 
  // 补偿 CarSim 坐标原点偏移
  FActorSpawnParameters SpawnParams;
  SpawnParams.SpawnCollisionHandlingOverride =
      ESpawnActorCollisionHandlingMethod::AlwaysSpawn;
  OffsetActor = GetWorld()->SpawnActor<AEmptyActor>(
      CarlaVehicle->GetActorLocation() + CarlaVehicle->GetActorForwardVector() * CarlaVehicle->CarSimOriginOffset,
      CarlaVehicle->GetActorRotation(),
      SpawnParams);
  CarSimMovementComponent = NewObject<UCarSimMovementComponent>(OffsetActor);
 
  carla::log_warning("Loading simfile:", carla::rpc::FromFString(SimfilePath));
  CarlaVehicle->GetVehicleMovementComponent()->SetComponentTickEnabled(false);
  CarlaVehicle->GetVehicleMovementComponent()->Deactivate();
  CarSimMovementComponent->DisableVehicle = false;
  CarSimMovementComponent->VsConfigFile = SimfilePath;
  CarSimMovementComponent->Activate();
  CarSimMovementComponent->RegisterComponent();
 
  CarSimMovementComponent->ResetVsVehicle(false);
  // 将 CarSim 的位置设置为实体的位置
  CarSimMovementComponent->SyncVsVehicleLocOri();
  CarSimMovementComponent->SetComponentTickEnabled(true);
  // 为根组件和骨骼设置运动模式
  CarlaVehicle->GetMesh()->PhysicsTransformUpdateMode = EPhysicsTransformUpdateMode::ComponentTransformIsKinematic;
  auto * Bone = CarlaVehicle->GetMesh()->GetBodyInstance(NAME_None);
  if (Bone)
  {
    Bone->SetInstanceSimulatePhysics(false);
  }
  // 设置回调以响应碰撞事件
  CarlaVehicle->OnActorHit.AddDynamic(this, &UCarSimManagerComponent::OnCarSimHit);
  CarlaVehicle->GetMesh()->OnComponentBeginOverlap.AddDynamic(this, &UCarSimManagerComponent::OnCarSimOverlap);
  CarlaVehicle->GetMesh()->OnComponentEndOverlap.AddDynamic(this, &UCarSimManagerComponent::OnCarSimEndOverlap);
  CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Overlap);
 
  // 解决方法：防止 CarSim 与其自身车辆发生交互
  CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(ECollisionChannel::ECC_Visibility, ECollisionResponse::ECR_Overlap);
 
  // 以偏移量附加到实体
  CarlaVehicle->AttachToActor(OffsetActor, FAttachmentTransformRules::KeepWorldTransform);
  #else
  UE_LOG(LogCarla, Warning, TEXT("Error: CarSim plugin is not enabled") );
  return;
  #endif
}
 
void UCarSimManagerComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
}
 
void UCarSimManagerComponent::ProcessControl(FVehicleControl &Control)
{
  #ifdef WITH_CARSIM
  CarSimMovementComponent->SetThrottleInput(Control.Throttle);
  CarSimMovementComponent->SetSteeringInput(Control.Steer);
  CarSimMovementComponent->SetBrakeInput(Control.Brake);
  if (Control.bHandBrake)
  {
    CarSimMovementComponent->SetBrakeInput(Control.Brake + 1.0);
  }
  Control.Gear = CarSimMovementComponent->GetCurrentGear();
  #endif
}
 
void UCarSimManagerComponent::DisableCarSimPhysics()
{
  #ifdef WITH_CARSIM
  // 结束 CarSim 仿真
  UDefaultMovementComponent::CreateDefaultMovementComponent(CarlaVehicle);
  CarlaVehicle->GetMesh()->SetPhysicsLinearVelocity(FVector(0,0,0), false, "Vehicle_Base");
  CarlaVehicle->GetVehicleMovementComponent()->SetComponentTickEnabled(true);
  CarlaVehicle->GetVehicleMovementComponent()->Activate();
  CarlaVehicle->GetMesh()->PhysicsTransformUpdateMode = EPhysicsTransformUpdateMode::SimulationUpatesComponentTransform;
  auto * Bone = CarlaVehicle->GetMesh()->GetBodyInstance(NAME_None);
  if (Bone)
  {
    Bone->SetInstanceSimulatePhysics(true);
  }
  else
  {
    carla::log_warning("No bone with name");
  }
  CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
  CarlaVehicle->GetMesh()->SetCollisionProfileName("Vehicle");    
  CarlaVehicle->RestoreVehiclePhysicsControl();
  #endif
}
 
void UCarSimManagerComponent::DisableSpecialPhysics()
{
  DisableCarSimPhysics();
}
 
void UCarSimManagerComponent::OnCarSimHit(AActor *Actor,
    AActor *OtherActor,
    FVector NormalImpulse,
    const FHitResult &Hit)
{
  DisableCarSimPhysics();
}
 
 
void UCarSimManagerComponent::OnCarSimOverlap(UPrimitiveComponent* OverlappedComponent,
    AActor* OtherActor,
    UPrimitiveComponent* OtherComp,
    int32 OtherBodyIndex,
    bool bFromSweep,
    const FHitResult & SweepResult)
{
  if (OtherComp->GetCollisionResponseToChannel(
      ECollisionChannel::ECC_WorldDynamic) ==
      ECollisionResponse::ECR_Block)
  {
    DisableCarSimPhysics();
  }
}
 
void UCarSimManagerComponent::OnCarSimEndOverlap(UPrimitiveComponent* OverlappedComponent,
    AActor* OtherActor,
    UPrimitiveComponent* OtherComp,
    int32 OtherBodyIndex)
{
}
 
void UCarSimManagerComponent::UseCarSimRoad(bool bEnabled)
{
  #ifdef WITH_CARSIM
  CarSimMovementComponent->UseVehicleSimRoad = bEnabled;
  CarSimMovementComponent->ResetVsVehicle(false);
  CarSimMovementComponent->SyncVsVehicleLocOri();
  #endif
}
 
 
void UCarSimManagerComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
  #ifdef WITH_CARSIM
  if (OffsetActor)
  {
    OffsetActor->Destroy();
  }
  
  if(!CarlaVehicle)
  {
    UE_LOG(LogCarla, Warning, TEXT("Error: Owner is not properly set for UCarSimManagerComponent") );
    return;
  }
 
  // 重置回调以响应碰撞事件
  CarlaVehicle->OnActorHit.RemoveDynamic(this, &UCarSimManagerComponent::OnCarSimHit);
  CarlaVehicle->GetMesh()->OnComponentBeginOverlap.RemoveDynamic(this, &UCarSimManagerComponent::OnCarSimOverlap);
  CarlaVehicle->GetMesh()->OnComponentEndOverlap.RemoveDynamic(this, &UCarSimManagerComponent::OnCarSimEndOverlap);
 
  #endif
}
 
FVector UCarSimManagerComponent::GetVelocity() const
{
  #ifdef WITH_CARSIM
  return GetOwner()->GetActorForwardVector() * CarSimMovementComponent->GetForwardSpeed();
  #else
  return FVector();
  #endif
}
 
int32 UCarSimManagerComponent::GetVehicleCurrentGear() const
{
  #ifdef WITH_CARSIM
  return CarSimMovementComponent->GetCurrentGear();
  #else
  return 0;
  #endif
}
 
float UCarSimManagerComponent::GetVehicleForwardSpeed() const
{
  #ifdef WITH_CARSIM
  return CarSimMovementComponent->GetForwardSpeed();
  #else
  return 0.0;
  #endif
}