diff --git a/AirSim/AirLib/include/api/RpcLibClientBase.hpp b/AirSim/AirLib/include/api/RpcLibClientBase.hpp index 79e064ce..9c14ca7d 100644 --- a/AirSim/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirSim/AirLib/include/api/RpcLibClientBase.hpp @@ -81,7 +81,6 @@ class RpcLibClientBase { // sensor omniscient APIs vector 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& color_rgba, float thickness=3.0f, const std::string& vehicle_name = ""); diff --git a/AirSim/AirLib/include/api/VehicleSimApiBase.hpp b/AirSim/AirLib/include/api/VehicleSimApiBase.hpp index 67904612..0269583b 100644 --- a/AirSim/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirSim/AirLib/include/api/VehicleSimApiBase.hpp @@ -47,7 +47,6 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject { virtual std::vector getImages(const std::vector& request) const = 0; virtual std::vector 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; diff --git a/AirSim/AirLib/src/api/RpcLibClientBase.cpp b/AirSim/AirLib/src/api/RpcLibClientBase.cpp index c5776043..bc71bcf5 100644 --- a/AirSim/AirLib/src/api/RpcLibClientBase.cpp +++ b/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().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); diff --git a/AirSim/AirLib/src/api/RpcLibServerBase.cpp b/AirSim/AirLib/src/api/RpcLibServerBase.cpp index 4d6887f9..f60f2814 100644 --- a/AirSim/AirLib/src/api/RpcLibServerBase.cpp +++ b/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& color_rgba, float thickness, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness); diff --git a/AirSim/PythonClient/airsim/client.py b/AirSim/PythonClient/airsim/client.py index 8ae3280e..40aa6adc 100644 --- a/AirSim/PythonClient/airsim/client.py +++ b/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 diff --git a/AirSim/PythonClient/computer_vision/cv_capture.py b/AirSim/PythonClient/computer_vision/cv_capture.py index 1ee625f8..e280e443 100644 --- a/AirSim/PythonClient/computer_vision/cv_capture.py +++ b/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) diff --git a/AirSim/PythonClient/computer_vision/cv_mode.py b/AirSim/PythonClient/computer_vision/cv_mode.py index 188080ba..f0de73ad 100644 --- a/AirSim/PythonClient/computer_vision/cv_mode.py +++ b/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) diff --git a/AirSim/PythonClient/computer_vision/getpos.py b/AirSim/PythonClient/computer_vision/getpos.py index 8304d429..5fb18bf3 100644 --- a/AirSim/PythonClient/computer_vision/getpos.py +++ b/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 diff --git a/UE4Project/Config/DefaultEngine.ini b/UE4Project/Config/DefaultEngine.ini index ad502658..19f1365a 100644 --- a/UE4Project/Config/DefaultEngine.ini +++ b/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 diff --git a/UE4Project/Plugins/AirSim/Source/PawnEvents.cpp b/UE4Project/Plugins/AirSim/Source/PawnEvents.cpp index b1896bc1..b00e00d3 100644 --- a/UE4Project/Plugins/AirSim/Source/PawnEvents.cpp +++ b/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_; } \ No newline at end of file diff --git a/UE4Project/Plugins/AirSim/Source/PawnEvents.h b/UE4Project/Plugins/AirSim/Source/PawnEvents.h index 164b69dc..06d912e9 100644 --- a/UE4Project/Plugins/AirSim/Source/PawnEvents.h +++ b/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_; + }; diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp index 4b630d6d..c582d580 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp +++ b/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(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(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() diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h index 72980272..258e02a2 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h +++ b/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; + }; diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index c110f6e6..5131c28f 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/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(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(); } diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index eb2f2d3c..f513d5b3 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/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 getImages(const std::vector& request) const override; virtual std::vector 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: