Added continueForFrames

AirLib/include/api/RpcLibClientBase.hpp

@@ -41,6 +41,7 @@ class RpcLibClientBase {
     bool simIsPaused() const;
     void simPause(bool is_paused);
     void simContinueForTime(double seconds);
+    void simContinueForFrames(uint32_t frames);
 
     void simSetTimeOfDay(bool is_enabled, const string& start_datetime = "", bool is_start_datetime_dst = false,
         float celestial_clock_speed = 1, float update_interval_secs = 60, bool move_sun = true);

AirLib/include/api/WorldSimApiBase.hpp

@@ -34,6 +34,7 @@ class WorldSimApiBase {
     virtual void reset() = 0;
     virtual void pause(bool is_paused) = 0;
     virtual void continueForTime(double seconds) = 0;
+    virtual void continueForFrames(uint32_t frames) = 0;
 
     virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
         float celestial_clock_speed, float update_interval_secs, bool move_sun) = 0;

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -63,6 +63,18 @@ class ScheduledExecutor {
         paused_ = false;
     }
 
+    void continueForFrames(uint32_t frames)
+    {
+        frame_countdown_enabled_ = true;
+        targetFrameNumber_ = frames + currentFrameNumber_;
+        paused_ = false;
+    }
+
+    void setFrameNumber(uint32_t frameNumber)
+    {
+        currentFrameNumber_ = frameNumber;    
+    }
+
     void stop()
     {
         if (started_) {
@@ -150,6 +162,15 @@ class ScheduledExecutor {
         while (started_) {
             TTimePoint period_start = nanos();
             TTimeDelta since_last_call = period_start - call_end;
+
+            if (frame_countdown_enabled_) {
+                if (targetFrameNumber_ <= currentFrameNumber_){
+                    if (! isPaused())
+                        pause(true);
+
+                    frame_countdown_enabled_ = false;
+                }
+            }
 
             if (pause_period_start_ > 0) {
                 if (nanos() - pause_period_start_ >= pause_period_) {
@@ -196,6 +217,9 @@ class ScheduledExecutor {
     std::atomic_bool paused_;
     std::atomic<TTimeDelta> pause_period_;
     std::atomic<TTimePoint> pause_period_start_;
+    uint32_t currentFrameNumber_;
+    uint32_t targetFrameNumber_;
+    std::atomic_bool frame_countdown_enabled_;
 
     double sleep_time_avg_;
 

AirLib/include/physics/PhysicsWorld.hpp

@@ -91,6 +91,16 @@ class PhysicsWorld {
         world_.continueForTime(seconds);
     }
 
+    void continueForFrames(uint32_t frames)
+    {
+        world_.continueForFrames(frames);
+    }
+
+    void setFrameNumber(uint32_t frameNumber)
+    {
+        world_.setFrameNumber(frameNumber);
+    }
+
 private:
     void initializeWorld(const std::vector<UpdatableObject*>& bodies, bool start_async_updator)
     {

AirLib/include/physics/World.hpp

@@ -123,6 +123,16 @@ class World : public UpdatableContainer<UpdatableObject*> {
         executor_.continueForTime(seconds);
     }
 
+    void continueForFrames(uint32_t frames)
+    {
+        executor_.continueForFrames(frames);
+    }
+
+    void setFrameNumber(uint32_t frameNumber)
+    {
+        executor_.setFrameNumber(frameNumber);
+    }
+
 private:
     bool worldUpdatorAsync(uint64_t dt_nanos)
     {

AirLib/src/api/RpcLibClientBase.cpp

@@ -325,6 +325,11 @@ void RpcLibClientBase::simContinueForTime(double seconds)
     pimpl_->client.call("simContinueForTime", seconds);
 }
 
+void RpcLibClientBase::simContinueForFrames(uint32_t frames)
+{
+    pimpl_->client.call("simContinueForFrames", frames);
+}
+
 void RpcLibClientBase::simEnableWeather(bool enable)
 {
     pimpl_->client.call("simEnableWeather", enable);

AirLib/src/api/RpcLibServerBase.cpp

@@ -104,6 +104,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         getWorldSimApi()->continueForTime(seconds); 
     });
 
+    pimpl_->server.bind("simContinueForFrames", [&](uint32_t frames) -> void { 
+        getWorldSimApi()->continueForFrames(frames); 
+    });
+
     pimpl_->server.bind("simSetTimeOfDay", [&](bool is_enabled, const string& start_datetime, bool is_start_datetime_dst, 
         float celestial_clock_speed, float update_interval_secs, bool move_sun) -> void {
         getWorldSimApi()->setTimeOfDay(is_enabled, start_datetime, is_start_datetime_dst, 

PythonClient/airsim/client.py

@@ -110,6 +110,15 @@ def simContinueForTime(self, seconds):
             seconds (float): Time to run the simulation for
         """
         self.client.call('simContinueForTime', seconds)
+
+    def simContinueForFrames(self, frames):
+        """
+        Continue the simulation for the specified number of frames
+
+        Args:
+            frames (float): Frames to run the simulation for
+        """
+        self.client.call('simContinueForFrames', frames)
 
     def getHomeGeoPoint(self, vehicle_name = ''):
         """

Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp

@@ -269,6 +269,13 @@ void ASimModeBase::continueForTime(double seconds)
     throw std::domain_error("continueForTime is not implemented by SimMode");
 }
 
+void ASimModeBase::continueForFrames(uint32_t frames)
+{
+    //should be overriden by derived class
+    unused(frames);
+    throw std::domain_error("continueForFrames is not implemented by SimMode");
+}
+
 void ASimModeBase::setWind(const msr::airlib::Vector3r& wind) const
 {
     // should be overridden by derived class

Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h

@@ -61,6 +61,7 @@ class AIRSIM_API ASimModeBase : public AActor
     virtual bool isPaused() const;
     virtual void pause(bool is_paused);
     virtual void continueForTime(double seconds);
+    virtual void continueForFrames(uint32_t frames);
 
     virtual void setWind(const msr::airlib::Vector3r& wind) const;
 

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp

@@ -100,6 +100,23 @@ void ASimModeWorldBase::continueForTime(double seconds)
     UGameplayStatics::SetGamePaused(this->GetWorld(), true);
 }
 
+void ASimModeWorldBase::continueForFrames(uint32_t frames)
+{
+    if(physics_world_->isPaused())
+    {
+        physics_world_->pause(false);
+        UGameplayStatics::SetGamePaused(this->GetWorld(), false);        
+    }
+
+    physics_world_->setFrameNumber((uint32_t)GFrameNumber);
+    physics_world_->continueForFrames(frames);
+    while(!physics_world_->isPaused())
+    {
+        physics_world_->setFrameNumber((uint32_t)GFrameNumber);
+    }
+    UGameplayStatics::SetGamePaused(this->GetWorld(), true);
+}
+
 void ASimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const
 {
     physics_engine_->setWind(wind);

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h

@@ -12,7 +12,7 @@
 #include "SimModeBase.h"
 #include "SimModeWorldBase.generated.h"
 
-
+extern CORE_API uint32 GFrameNumber;
 
 UCLASS()
 class AIRSIM_API ASimModeWorldBase : public ASimModeBase
@@ -30,6 +30,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase
     virtual bool isPaused() const override;
     virtual void pause(bool is_paused) override;
     virtual void continueForTime(double seconds) override;
+    virtual void continueForFrames(uint32_t frames) override;
 
     virtual void setWind(const msr::airlib::Vector3r& wind) const override;
 

Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp

@@ -9,6 +9,7 @@
 #include "common/EarthUtils.hpp"
 #include "vehicles/car/api/CarRpcLibServer.hpp"
 
+extern CORE_API uint32 GFrameNumber;
 
 void ASimModeCar::BeginPlay()
 {
@@ -46,6 +47,13 @@ void ASimModeCar::continueForTime(double 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;
@@ -67,6 +75,15 @@ void ASimModeCar::Tick(float DeltaSeconds)
             pause_period_start_ = 0;
         }
     }
+
+    if(frame_countdown_enabled_){
+        if (targetFrameNumber_ <= GFrameNumber){
+            if (! isPaused())
+                pause(true);
+
+            frame_countdown_enabled_ = false;
+        }
+    }
 }
 
 //-------------------------------- overrides -----------------------------------------------//

Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.h

@@ -21,6 +21,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase
     virtual bool isPaused() const override;
     virtual void pause(bool is_paused) override;
     virtual void continueForTime(double seconds) override;
+    virtual void continueForFrames(uint32_t frames) override;
 
 private:
     typedef msr::airlib::ClockFactory ClockFactory;
@@ -53,4 +54,6 @@ class AIRSIM_API ASimModeCar : public ASimModeBase
     std::atomic<float> current_clockspeed_;
     std::atomic<TTimeDelta> pause_period_;
     std::atomic<TTimePoint> pause_period_start_;
+    uint32_t targetFrameNumber_;
+    std::atomic_bool frame_countdown_enabled_;;
 };

Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

@@ -183,6 +183,11 @@ void WorldSimApi::continueForTime(double seconds)
     simmode_->continueForTime(seconds);
 }
 
+void WorldSimApi::continueForFrames(uint32_t frames)
+{
+    simmode_->continueForFrames(frames);
+}
+
 void WorldSimApi::setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
     float celestial_clock_speed, float update_interval_secs, bool move_sun)
 {

Unreal/Plugins/AirSim/Source/WorldSimApi.h

@@ -27,6 +27,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase {
     virtual void reset() override;
     virtual void pause(bool is_paused) override;
     virtual void continueForTime(double seconds) override;
+    virtual void continueForFrames(uint32_t frames) override;
 
     virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
         float celestial_clock_speed, float update_interval_secs, bool move_sun);