AirSim中的物理引擎

“物理引擎”是指在一款仿真软件/平台中使用的模拟现实世界物理逻辑（如碰撞、各种有形变/无形变的力学等）的计算单元，例如在开源机器人仿真平台gazebo中，就支持如ODE、Bullets、SimBody等多种物理引擎。

在AirSim中，针对无人机MultiRotor的物理引擎是AirSim自己开发的FastPhysics，而针对车辆Car的物理引擎是Unreal自己的PhysX引擎。

我不知道AirSim团队进行这样设计的初衷是什么，我推测可能是FastPhysics是针对无人机开发的一款比较快速的物理引擎，毕竟可能无人机的物理模型比车辆要更复杂一点。但我不知道为什么他们不顺便把车辆的仿真模式中也用上自己的FastPhysics，我估计是因为Unreal的PhysX对车辆模拟来说已经足够好了，并且使用上也方便一些。

• 有关PhysX的简要介绍：UE4物理系统浅析
• FastPhysics的定义在/AirSim/AirLib/include/physics/FastPhysicsEngine.hpp中

但这样做会产生一个问题，那就是AirSim中的settings中的ClockSpeed参数只对无人机MultiRotor SimMode有效，在车辆Car SimMode下无法有效调节仿真速率，具体问题描述可以参见：微软开源仿真器AirSim ROS接口中的第2节。

在车辆模式应用物理引擎的/AirSim/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp文件中，原本是这样写的：

#include "SimModeCar.h"
#include "UObject/ConstructorHelpers.h"

#include "AirBlueprintLib.h"
#include "common/AirSimSettings.hpp"
#include "CarPawnSimApi.h"
#include "AirBlueprintLib.h"
#include "common/Common.hpp"
#include "common/EarthUtils.hpp"
#include "vehicles/car/api/CarRpcLibServer.hpp"

extern CORE_API uint32 GFrameNumber;

void ASimModeCar::BeginPlay()
{
    Super::BeginPlay();

    initializePauseState();
}

void ASimModeCar::initializePauseState()
{
    pause_period_ = 0;
    pause_period_start_ = 0;
    pause(false);
}

bool ASimModeCar::isPaused() const
{
    return current_clockspeed_ == 0;
}

void ASimModeCar::pause(bool is_paused)
{
    if (is_paused)
        current_clockspeed_ = 0;
    else
        current_clockspeed_ = getSettings().clock_speed;

    UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_);
}

void ASimModeCar::continueForTime(double seconds)
{
    pause_period_start_ = ClockFactory::get()->nowNanos();
    pause_period_ = seconds;
    pause(false);
}

void ASimModeCar::continueForFrames(uint32_t frames)
{
    targetFrameNumber_ = GFrameNumber + frames;
    frame_countdown_enabled_ = true;
    pause(false);
}

void ASimModeCar::setupClockSpeed()
{
    current_clockspeed_ = getSettings().clock_speed;

    //setup clock in PhysX
    //在这里我们可以看到，代码直接使用了UAirBlueprintLib的相关方法，也就是使用了Unreal的默认引擎PhysX（目前Unreal新增了Chaos引擎，不知道AirSim以后会不会向Chaos兼容）
    //值得注意的是，在这里的确是设定了clockspeed，但效果仅仅是使整个仿真在”人的感官“下产生了速度变化，
    //仿真输出的时间戳没有变化，不像FastPhysicsEngine那样，仿真输出的时间戳也会相应变化。
    UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_);
    UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(current_clockspeed_), LogDebugLevel::Informational);
}

void ASimModeCar::Tick(float DeltaSeconds)
{
    Super::Tick(DeltaSeconds);
    
    if (pause_period_start_ > 0) {
        if (ClockFactory::get()->elapsedSince(pause_period_start_) >= pause_period_) {
            if (!isPaused())
                pause(true);

            pause_period_start_ = 0;
        }
    }

    if(frame_countdown_enabled_){
        if (targetFrameNumber_ <= GFrameNumber){
            if (! isPaused())
                pause(true);

            frame_countdown_enabled_ = false;
        }
    }
}

//-------------------------------- overrides -----------------------------------------------//
//以下略。

代码中的

UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_);

目的是设置clockspeed，但是对Unreal的PhysX而言，仅仅是使仿真速度在“人的感官”下产生了变化，而仿真输出的信息的时间戳并没有随着ClockSpeed参数的设置而产生更“稀疏”或更“稠密”的变化。

所以我对此文件进行了改写，使车辆模式的仿真也可以使用MultiRotor的FastPhysicsEngine，直接上代码：

#include "SimModeCar.h"
#include "UObject/ConstructorHelpers.h"
#include "Logging/MessageLog.h"
#include "Engine/World.h"
#include "GameFramework/PlayerController.h"
#include <exception>
#include "AirBlueprintLib.h"
#include "common/AirSimSettings.hpp"
#include "CarPawnSimApi.h"
#include "physics/PhysicsBody.hpp"
#include "common/ClockFactory.hpp"
#include "common/EarthUtils.hpp"
#include "common/Common.hpp"
#include <memory>
#include "vehicles/car/api/CarRpcLibServer.hpp"


void ASimModeCar::BeginPlay()
{
    Super::BeginPlay();

    //let base class setup physics world
    initializeForPlay();
}
void ASimModeCar::initializeForPlay()
{
    std::vector<msr::airlib::UpdatableObject*> vehicles;
    for (auto& api : getApiProvider()->getVehicleSimApis())
        vehicles.push_back(api);
    //TODO: directly accept getVehicleSimApis() using generic container

    std::unique_ptr<PhysicsEngineBase> physics_engine = createPhysicsEngine();
    physics_engine_ = physics_engine.get();
    physics_world_.reset(new msr::airlib::PhysicsWorld(std::move(physics_engine),
        vehicles, getPhysicsLoopPeriod()));
}
void ASimModeCar::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    //remove everything that we created in BeginPlay
    physics_world_.reset();
    //stop physics thread before we dismantle
    //stopAsyncUpdator();

    Super::EndPlay(EndPlayReason);
}
void ASimModeCar::startAsyncUpdator()
{
    physics_world_->startAsyncUpdator();
}

void ASimModeCar::stopAsyncUpdator()
{
    physics_world_->stopAsyncUpdator();
}

long long ASimModeCar::getPhysicsLoopPeriod() const //nanoseconds
{
    return physics_loop_period_;
}
void ASimModeCar::setPhysicsLoopPeriod(long long  period)
{
    physics_loop_period_ = period;
}
std::unique_ptr<ASimModeCar::PhysicsEngineBase> ASimModeCar::createPhysicsEngine()
{
    std::unique_ptr<PhysicsEngineBase> physics_engine;
    std::string physics_engine_name = getSettings().physics_engine_name;
    //选择使用的物理引擎：
    if (physics_engine_name == "")
        physics_engine.reset(); //no physics engine
    else if (physics_engine_name == "FastPhysicsEngine") {
        msr::airlib::Settings fast_phys_settings;
        if (msr::airlib::Settings::singleton().getChild("FastPhysicsEngine", fast_phys_settings)) {
            physics_engine.reset(new msr::airlib::FastPhysicsEngine(fast_phys_settings.getBool("EnableGroundLock", true)));
        }
        else {
            physics_engine.reset(new msr::airlib::FastPhysicsEngine());
        }
    }
    else {
        physics_engine.reset();
        UAirBlueprintLib::LogMessageString("Unrecognized physics engine name: ",  physics_engine_name, LogDebugLevel::Failure);
    }

    return physics_engine;
}

bool ASimModeCar::isPaused() const
{
    return physics_world_->isPaused();
}

void ASimModeCar::pause(bool is_paused)
{
    physics_world_->pause(is_paused);
    UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused);
    pause_physx(true);
}
void ASimModeCar::pause_physx(bool is_paused)
{
    float current_clockspeed_;
    if (is_paused)
        current_clockspeed_ = 0;
    else
        current_clockspeed_ = getSettings().clock_speed;
    UE_LOG(LogTemp,Display,TEXT("*******check point pause:"));
    UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_);
}
void ASimModeCar::continueForTime(double seconds)
{
    if(physics_world_->isPaused())
    {
        physics_world_->pause(false);
        UGameplayStatics::SetGamePaused(this->GetWorld(), false);        
        pause_physx(false);
    }

    physics_world_->continueForTime(seconds);
    while(!physics_world_->isPaused())
    {
        continue; 
    }
    UGameplayStatics::SetGamePaused(this->GetWorld(), true);
}

void ASimModeCar::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter)
{
    unused(debug_reporter);
    //we use custom debug reporting for this class
}

void ASimModeCar::Tick(float DeltaSeconds)
{
    { //keep this lock as short as possible
        physics_world_->lock();

        physics_world_->enableStateReport(EnableReport);
        physics_world_->updateStateReport();

        for (auto& api : getApiProvider()->getVehicleSimApis())
            api->updateRenderedState(DeltaSeconds);

        physics_world_->unlock();
    }

    //perform any expensive rendering update outside of lock region
    for (auto& api : getApiProvider()->getVehicleSimApis())
        api->updateRendering(DeltaSeconds);

    Super::Tick(DeltaSeconds);
}

void ASimModeCar::reset()
{
    UAirBlueprintLib::RunCommandOnGameThread([this]() {
        physics_world_->reset();
    }, true);
    
    //no need to call base reset because of our custom implementation
}

std::string ASimModeCar::getDebugReport()
{
    return physics_world_->getDebugReport();
}
void ASimModeCar::setupClockSpeed()
{
    typedef msr::airlib::ClockFactory ClockFactory;

    float clock_speed = getSettings().clock_speed;

    float printout = getSettings().clock_speed;
    //setup clock in PhysX 
    UAirBlueprintLib::setUnrealClockSpeed(this, clock_speed);
    UAirBlueprintLib::LogMessageString("Clock Speed: ", std::to_string(clock_speed), LogDebugLevel::Informational);
    UE_LOG(LogTemp,Display,TEXT("*******check point 3: %f"),printout);
    //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<msr::airlib::ScalableClock>(clock_speed == 1 ? 1 : 1 / clock_speed));
        UE_LOG(LogTemp,Display,TEXT("*******check point 3_1: %f"),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<msr::airlib::SteppableClock>(
        //static_cast<msr::airlib::TTimeDelta>(getPhysicsLoopPeriod() * 1E-9 * clock_speed)));

        //Approach 2: scale control loop frequency if clock is speeded up
        UE_LOG(LogTemp,Display,TEXT("*******check point 3_2: %f"),clock_speed);
        if (clock_speed >= 1) {
            ClockFactory::get(std::make_shared<msr::airlib::SteppableClock>(
                static_cast<msr::airlib::TTimeDelta>(getPhysicsLoopPeriod() * 1E-9))); //no clock_speed multiplier

            setPhysicsLoopPeriod(getPhysicsLoopPeriod() / static_cast<long long>(clock_speed));
        }
        else {
            //for slowing down, this don't generate instability
            ClockFactory::get(std::make_shared<msr::airlib::SteppableClock>(
                static_cast<msr::airlib::TTimeDelta>(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 -----------------------------------------------//
//以下略

显然对应的hpp也需要修改，全部改动后的代码在：GimpelZhang/AirSim