High tickrate attempt 2

AirSim/AirLib/include/api/RpcLibClientBase.hpp

@@ -81,7 +81,6 @@ class RpcLibClientBase {
     // sensor omniscient APIs
     vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
 
-    Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
     void simSetTraceLine(const std::vector<float>& color_rgba, float thickness=3.0f, const std::string& vehicle_name = "");
 

AirSim/AirLib/include/api/VehicleSimApiBase.hpp

@@ -47,7 +47,6 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual std::vector<ImageCaptureBase::ImageResponse> getImages(const std::vector<ImageCaptureBase::ImageRequest>& request) const = 0;
     virtual std::vector<uint8_t> getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const = 0;
 
-    virtual Pose getPose() const = 0;
     virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
     virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
 

AirSim/AirLib/src/api/RpcLibClientBase.cpp

@@ -196,10 +196,6 @@ CollisionInfo RpcLibClientBase::simGetCollisionInfo(const std::string& vehicle_n
 
 
 //sim only
-Pose RpcLibClientBase::simGetVehiclePose(const std::string& vehicle_name) const
-{
-    return pimpl_->client.call("simGetVehiclePose", vehicle_name).as<RpcLibAdapatorsBase::Pose>().to();
-}
 void RpcLibClientBase::simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name)
 {
     pimpl_->client.call("simSetVehiclePose", RpcLibAdapatorsBase::Pose(pose), ignore_collision, vehicle_name);

AirSim/AirLib/src/api/RpcLibServerBase.cpp

@@ -145,10 +145,6 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void {
         getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision);
     });
-    pimpl_->server.bind("simGetVehiclePose", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::Pose {
-        const auto& pose = getVehicleSimApi(vehicle_name)->getPose();
-        return RpcLibAdapatorsBase::Pose(pose);
-    });
 
     pimpl_->server.bind("simSetTraceLine", [&](const std::vector<float>& color_rgba, float thickness, const std::string& vehicle_name) -> void {
         getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness);

AirSim/PythonClient/airsim/client.py

@@ -291,17 +291,6 @@ def simSetVehiclePose(self, pose, ignore_collison, vehicle_name = ''):
         """
         self.client.call('simSetVehiclePose', pose, ignore_collison, vehicle_name)
 
-    def simGetVehiclePose(self, vehicle_name = ''):
-        """
-        Args:
-            vehicle_name (str, optional): Name of the vehicle to get the Pose of
-
-        Returns:
-            Pose:
-        """
-        pose = self.client.call('simGetVehiclePose', vehicle_name)
-        return Pose.from_msgpack(pose)
-
     def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''):
         """
         Modify the color and thickness of the line when Tracing is enabled

AirSim/PythonClient/computer_vision/cv_capture.py

@@ -46,7 +46,6 @@
             print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position)))
             airsim.write_file(os.path.normpath(os.path.join(tmp_dir, str(i), str(x) + "_" + str(i) + '.png')), response.image_data_uint8)
 
-    pose = client.simGetVehiclePose()
     pp.pprint(pose)
 
     time.sleep(3)

AirSim/PythonClient/computer_vision/cv_mode.py

@@ -42,7 +42,6 @@
             print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position)))
             airsim.write_file(os.path.normpath('/temp/cv_mode_' + str(x) + "_" + str(i) + '.png'), response.image_data_uint8)
 
-    pose = client.simGetVehiclePose()
     pp.pprint(pose)
 
     time.sleep(3)

AirSim/PythonClient/computer_vision/getpos.py

@@ -7,10 +7,8 @@
 client = airsim.VehicleClient()
 client.confirmConnection()
 
-pose = client.simGetVehiclePose()
 print("x={}, y={}, z={}".format(pose.position.x_val, pose.position.y_val, pose.position.z_val))
 
-angles = airsim.to_eularian_angles(client.simGetVehiclePose().orientation)
 print("pitch={}, roll={}, yaw={}".format(angles[0], angles[1], angles[2]))
 
 pose.position.x_val = pose.position.x_val + 1

UE4Project/Config/DefaultEngine.ini

@@ -24,6 +24,7 @@ MinimumiOSVersion=IOS_11
 +ActiveGameNameRedirects=(OldGameName="/Script/TP_Flying",NewGameName="/Script/Blocks")
 +ActiveClassRedirects=(OldClassName="TP_FlyingPawn",NewClassName="BlocksPawn")
 +ActiveClassRedirects=(OldClassName="TP_FlyingGameMode",NewClassName="BlocksGameMode")
+bUseFixedFrameRate=True
 
 [/Script/HardwareTargeting.HardwareTargetingSettings]
 TargetedHardwareClass=Desktop
@@ -63,10 +64,10 @@ bDisableActiveActors=False
 bDisableCCD=False
 bEnableEnhancedDeterminism=False
 MaxPhysicsDeltaTime=0.033333
-bSubstepping=False
-bSubsteppingAsync=False
-MaxSubstepDeltaTime=0.016667
-MaxSubsteps=6
+bSubstepping=True
+bSubsteppingAsync=True
+MaxSubstepDeltaTime=0.010000
+MaxSubsteps=10
 SyncSceneSmoothingFactor=0.000000
 AsyncSceneSmoothingFactor=0.990000
 InitialAverageFrameRate=0.016667

UE4Project/Plugins/AirSim/Source/PawnEvents.cpp

@@ -9,4 +9,9 @@ PawnEvents::PawnTickSignal& PawnEvents::getPawnTickSignal()
 {
     return pawn_tick_signal_;
 
+}
+
+PawnEvents::PawnTickSignal& PawnEvents::getPawnSubtickSignal()
+{
+    return pawn_subtick_signal_;
 }

UE4Project/Plugins/AirSim/Source/PawnEvents.h

@@ -15,8 +15,11 @@ class PawnEvents {
 public:
     CollisionSignal& getCollisionSignal();
     PawnTickSignal& getPawnTickSignal();
+    PawnTickSignal& getPawnSubtickSignal();
 
 private:
     CollisionSignal collision_signal_;
     PawnTickSignal pawn_tick_signal_;
+    PawnTickSignal pawn_subtick_signal_;
+
 };

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp

@@ -76,6 +76,8 @@ ACarPawn::ACarPawn()
     last_gear_display_color_ = FColor(255, 255, 255, 255);
 
     is_low_friction_ = false;
+
+    OnCalculateCustomPhysics.BindUObject(this, &ACarPawn::SubstepTick);
 }
 
 void ACarPawn::setupVehicleMovementComponent()
@@ -161,6 +163,12 @@ UWheeledVehicleMovementComponent* ACarPawn::getVehicleMovementComponent() const
     return GetVehicleMovement();
 }
 
+FBodyInstance* ACarPawn::getBodyInstance() const
+{
+    UPrimitiveComponent* UpdatedPrimitive = Cast<UPrimitiveComponent>(GetVehicleMovementComponent()->UpdatedComponent);
+    return UpdatedPrimitive->GetBodyInstance();
+}
+
 void ACarPawn::initializeForBeginPlay()
 {
     //put camera little bit above vehicle
@@ -230,6 +238,8 @@ void ACarPawn::EndPlay(const EEndPlayReason::Type EndPlayReason)
 void ACarPawn::Tick(float Delta)
 {
     Super::Tick(Delta);
+    UE_LOG(LogTemp, Warning, TEXT("Tick %f"), Delta );
+
 
     // update physics material
     updatePhysicsMaterial();
@@ -241,6 +251,16 @@ void ACarPawn::Tick(float Delta)
     updateInCarHUD();
 
     pawn_events_.getPawnTickSignal().emit(Delta);
+
+    UPrimitiveComponent* UpdatedPrimitive = Cast<UPrimitiveComponent>(GetVehicleMovementComponent()->UpdatedComponent);
+
+    UpdatedPrimitive->GetBodyInstance()->AddCustomPhysics(OnCalculateCustomPhysics);
+}
+
+void ACarPawn::SubstepTick(float DeltaTime, FBodyInstance* BodyInstance)
+{
+    pawn_events_.getPawnSubtickSignal().emit(DeltaTime);
+    UE_LOG(LogTemp, Warning, TEXT("Substep Tick %f"), DeltaTime );
 }
 
 void ACarPawn::BeginPlay()

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h

@@ -35,6 +35,8 @@ class ACarPawn : public AWheeledVehicle
 
     virtual void BeginPlay() override;
     virtual void Tick(float Delta) override;
+    void SubstepTick(float DeltaTime, FBodyInstance* BodyInstance);
+
     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;
@@ -47,6 +49,8 @@ class ACarPawn : public AWheeledVehicle
         return &pawn_events_;
     }
     UWheeledVehicleMovementComponent* getVehicleMovementComponent() const;
+    FBodyInstance* getBodyInstance() const;
+
     const msr::airlib::CarApiBase::CarControls& getKeyBoardControls() const
     {
         return keyboard_controls_;
@@ -99,4 +103,7 @@ class ACarPawn : public AWheeledVehicle
     FText last_gear_;
     FColor	last_gear_display_color_;
     FColor	last_gear_display_reverse_color_;
+
+    FCalculateCustomPhysics OnCalculateCustomPhysics;
+
 };

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp

@@ -7,6 +7,7 @@
 #include "Kismet/KismetSystemLibrary.h"
 #include "Kismet/GameplayStatics.h"
 #include "Camera/CameraComponent.h"
+#include "WheeledVehicle.h"
 
 #include "common/ClockFactory.hpp"
 #include "PIPCamera.h"
@@ -27,7 +28,7 @@ CarPawnSimApi::CarPawnSimApi(const Params& params, const msr::airlib::CarApiBase
 void CarPawnSimApi::initialize()
 {
     Kinematics::State initial_kinematic_state = Kinematics::State::zero();;
-    initial_kinematic_state.pose = getPose();
+    initial_kinematic_state.pose = toPose(getUUPosition(), getUUOrientation().Quaternion());
     kinematics_.reset(new Kinematics(initial_kinematic_state));
 
     //initialize state
@@ -54,6 +55,8 @@ void CarPawnSimApi::initialize()
     //add listener for pawn's collision event
     params_.pawn_events->getCollisionSignal().connect_member(this, &CarPawnSimApi::onCollision);
     params_.pawn_events->getPawnTickSignal().connect_member(this, &CarPawnSimApi::pawnTick);
+    params_.pawn_events->getPawnSubtickSignal().connect_member(this, &CarPawnSimApi::pawnSubtick);
+
 
     createVehicleApi(static_cast<ACarPawn*>(params_.pawn), params_.home_geopoint);
 
@@ -76,6 +79,7 @@ void CarPawnSimApi::createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint
 void CarPawnSimApi::updateRenderedState(float dt)
 {
     updateKinematics(dt);
+
     vehicle_api_->getStatusMessages(vehicle_api_messages_);
 
     //TODO: do we need this for cars?
@@ -218,6 +222,13 @@ void CarPawnSimApi::pawnTick(float dt)
     updateRendering(dt);
 }
 
+void CarPawnSimApi::pawnSubtick(float dt)
+{
+    update();
+    updateKinematics(dt);
+    updateRendering(dt);
+}
+
 void CarPawnSimApi::detectUsbRc()
 {
     if (getRemoteControlID() >= 0) {
@@ -435,6 +446,22 @@ FRotator CarPawnSimApi::getUUOrientation() const
     return params_.pawn->GetActorRotation();
 }
 
+FVector CarPawnSimApi::getUUBodyPosition() const
+{
+    return pawn_->getBodyInstance()->GetUnrealWorldTransform_AssumesLocked().GetLocation();
+}
+
+FRotator CarPawnSimApi::getUUBodyOrientation() const
+{
+    return pawn_->getBodyInstance()->GetUnrealWorldTransform_AssumesLocked().Rotator();
+}
+
+FVector CarPawnSimApi::getUUBodyVelocity() const
+{
+    return pawn_->getBodyInstance()->GetUnrealWorldVelocity();
+}
+
+
 void CarPawnSimApi::toggleTrace()
 {
     state_.tracing_enabled = !state_.tracing_enabled;
@@ -511,10 +538,6 @@ void CarPawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degre
 }
 
 //parameters in NED frame
-CarPawnSimApi::Pose CarPawnSimApi::getPose() const
-{
-    return toPose(getUUPosition(), getUUOrientation().Quaternion());
-}
 
 CarPawnSimApi::Pose CarPawnSimApi::toPose(const FVector& u_position, const FQuat& u_quat) const
 {
@@ -577,14 +600,16 @@ void CarPawnSimApi::updateKinematics(float dt)
 
     auto next_kinematics = kinematics_->getState();
 
-    next_kinematics.pose = getPose();
-    next_kinematics.twist.linear = getNedTransform().toLocalNedVelocity(getPawn()->GetVelocity());
+    next_kinematics.pose = toPose(getUUBodyPosition(), getUUBodyOrientation().Quaternion());
+    next_kinematics.twist.linear = getNedTransform().toLocalNedVelocity(getUUBodyVelocity());
     next_kinematics.twist.angular = msr::airlib::VectorMath::toAngularVelocity(
         kinematics_->getPose().orientation, next_kinematics.pose.orientation, dt);
 
     next_kinematics.accelerations.linear = (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt;
     next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt;
 
+    UE_LOG(LogTemp, Warning, TEXT("updateKinematics DT: %f    X: %f    TLX: %f"), dt, next_kinematics.pose.position.x(), next_kinematics.twist.linear.x());
+
     kinematics_->setState(next_kinematics);
     kinematics_->update();
 }

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h

@@ -101,7 +101,6 @@ class CarPawnSimApi : public msr::airlib::VehicleSimApiBase
     virtual const UnrealImageCapture* getImageCapture() const override;
     virtual std::vector<ImageCaptureBase::ImageResponse> getImages(const std::vector<ImageCaptureBase::ImageRequest>& request) const override;
     virtual std::vector<uint8_t> getImage(const std::string& camera_name, ImageCaptureBase::ImageType image_type) const override;
-    virtual Pose getPose() const override;
     virtual void setPose(const Pose& pose, bool ignore_collision) override;
 
     virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override;
@@ -115,6 +114,12 @@ class CarPawnSimApi : public msr::airlib::VehicleSimApiBase
     FVector getUUPosition() const;
     FRotator getUUOrientation() const;
 
+    // The body position is updated every physics iteration, every subtick
+    // This is in contrast to the normal UUPosition and UUOrientation that is only updated every tick.
+    FVector getUUBodyPosition() const;
+    FRotator getUUBodyOrientation() const;
+    FVector getUUBodyVelocity() const;
+
     const NedTransform& getNedTransform() const;
     void possess();
     void setRCForceFeedback(float rumble_strength, float auto_center);
@@ -142,6 +147,8 @@ class CarPawnSimApi : public msr::airlib::VehicleSimApiBase
     void resetPawn();
     msr::airlib::Kinematics* getKinematics();
     virtual void pawnTick(float dt);
+    virtual void pawnSubtick(float dt);
+
     void setPoseInternal(const Pose& pose, bool ignore_collision);
 
 private: