diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 93de43cd27..ab082bd54c 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -45,7 +45,7 @@ class RpcLibClientBase { bool simIsPaused(); void simPause(bool is_paused); - void simContinueForTicks(uint32_t ticks); + void simContinueForTime(double seconds); virtual ~RpcLibClientBase(); //required for pimpl diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp index a1252b3a2e..6a6d4b3fc8 100644 --- a/AirLib/include/api/SimModeApiBase.hpp +++ b/AirLib/include/api/SimModeApiBase.hpp @@ -15,7 +15,7 @@ class SimModeApiBase { virtual VehicleApiBase* getVehicleApi() = 0; virtual bool isPaused() const = 0; virtual void pause(bool is_paused) = 0; - virtual void continueForTicks(uint32_t ticks) = 0; + virtual void continueForTime(double seconds) = 0; virtual ~SimModeApiBase() = default; }; diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp index 8ab9b60252..108146c5ec 100644 --- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp +++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp @@ -56,10 +56,10 @@ class ScheduledExecutor { return paused_; } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - pause_countdown_enabled_ = true; - pause_countdown_ = ticks; + pause_period_start_ = nanos(); + pause_period_ = static_cast(1E9 * seconds); paused_ = false; } @@ -105,8 +105,8 @@ class ScheduledExecutor { void initializePauseState() { paused_ = false; - pause_countdown_enabled_ = false; - pause_countdown_ = 0; + pause_period_start_ = 0; + pause_period_ = 0; } private: @@ -151,14 +151,12 @@ class ScheduledExecutor { TTimePoint period_start = nanos(); TTimeDelta since_last_call = period_start - call_end; - if (pause_countdown_enabled_) { - if (pause_countdown_ > 0) - --pause_countdown_; - else { - if (! paused_) - paused_ = true; - - pause_countdown_enabled_ = false; + if (pause_period_start_ > 0) { + if (nanos() - pause_period_start_ >= pause_period_) { + if (! isPaused()) + pause(true); + + pause_period_start_ = 0; } } @@ -196,8 +194,8 @@ class ScheduledExecutor { bool is_first_period_; std::atomic_bool started_; std::atomic_bool paused_; - std::atomic_uint32_t pause_countdown_; - std::atomic_bool pause_countdown_enabled_; + std::atomic pause_period_; + std::atomic pause_period_start_; double sleep_time_avg_; diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp index 9030b0677e..f397b5ed26 100644 --- a/AirLib/include/physics/PhysicsWorld.hpp +++ b/AirLib/include/physics/PhysicsWorld.hpp @@ -86,9 +86,9 @@ class PhysicsWorld { return world_.isPaused(); } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - world_.continueForTicks(ticks); + world_.continueForTime(seconds); } private: diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp index ad3f302883..5d9d03df64 100644 --- a/AirLib/include/physics/World.hpp +++ b/AirLib/include/physics/World.hpp @@ -118,9 +118,9 @@ class World : public UpdatableContainer { return executor_.isPaused(); } - void continueForTicks(uint32_t ticks) + void continueForTime(double seconds) { - executor_.continueForTicks(ticks); + executor_.continueForTime(seconds); } private: diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 3e4f777418..442e6fb62f 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -178,9 +178,9 @@ void RpcLibClientBase::simPause(bool is_paused) pimpl_->client.call("simPause", is_paused); } -void RpcLibClientBase::simContinueForTicks(uint32_t ticks) +void RpcLibClientBase::simContinueForTime(double seconds) { - pimpl_->client.call("simContinueForTicks", ticks); + pimpl_->client.call("simContinueForTime", seconds); } msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 212be6ba43..6d208bcc00 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -128,8 +128,8 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad pimpl_->server.bind("simIsPaused", [&]() -> bool { return getSimModeApi()->isPaused(); }); - pimpl_->server.bind("simContinueForTicks", [&](uint32_t ticks) -> void { - getSimModeApi()->continueForTicks(ticks); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { + getSimModeApi()->continueForTime(seconds); }); pimpl_->server.suppress_exceptions(true); diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py index 97b6ea896c..00b76f84ef 100644 --- a/PythonClient/AirSimClient.py +++ b/PythonClient/AirSimClient.py @@ -500,8 +500,8 @@ def simIsPause(self): return self.client.call("simIsPaused") def simPause(self, is_paused): self.client.call('simPause', is_paused) - def simContinueForTicks(self, ticks): - self.client.call('simContinueForTicks', int(ticks)) + def simContinueForTime(self, seconds): + self.client.call('simContinueForTime', seconds) # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(AirSimClientBase, object): diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj index 653d5b1142..f659f7df69 100644 --- a/PythonClient/PythonClient.pyproj +++ b/PythonClient/PythonClient.pyproj @@ -5,7 +5,7 @@ 2.0 e2049e20-b6dd-474e-8bca-1c8dc54725aa . - pause_continue_car.py + hello_car.py . diff --git a/PythonClient/pause_continue_car.py b/PythonClient/pause_continue_car.py index 312f3f93e0..c6abd7ca29 100644 --- a/PythonClient/pause_continue_car.py +++ b/PythonClient/pause_continue_car.py @@ -17,7 +17,7 @@ client.simPause(True) time.sleep(5) #paused print("Restarting command to run for 10sec") - client.simContinueForTicks(600) #600 ticks of 1/60sec each = 10sec + client.simContinueForTime(10) time.sleep(20) print("Finishing rest of the command") client.simPause(False) diff --git a/PythonClient/pause_continue_drone.py b/PythonClient/pause_continue_drone.py index 8abaf49264..de93168ffc 100644 --- a/PythonClient/pause_continue_drone.py +++ b/PythonClient/pause_continue_drone.py @@ -17,12 +17,12 @@ print("Pausing after 5sec") client.simPause(True) time.sleep(5) #paused - print("Restarting command to run for 6sec") - client.simContinueForTicks(2000) #2000 ticks of 3ms each = 1.5sec + print("Restarting command to run for 7.5sec") + client.simContinueForTime(7.5) time.sleep(10) print("Finishing rest of the command") client.simPause(False) - time.sleep(10) + time.sleep(15) print("Finished cycle") diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp index 2249d1bd63..c25528c349 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.cpp @@ -56,8 +56,8 @@ VehiclePawnWrapper* ASimModeCar::getFpvVehiclePawnWrapper() const void ASimModeCar::initializePauseState() { - pause_countdown_ = 0; - pause_countdown_enabled_ = false; + pause_period_ = 0; + pause_period_start_ = 0; pause(false); } @@ -76,10 +76,10 @@ void ASimModeCar::pause(bool is_paused) UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_); } -void ASimModeCar::continueForTicks(uint32_t ticks) +void ASimModeCar::continueForTime(double seconds) { - pause_countdown_enabled_ = true; - pause_countdown_ = ticks; + pause_period_start_ = ClockFactory::get()->nowNanos(); + pause_period_ = seconds; pause(false); } @@ -222,14 +222,12 @@ void ASimModeCar::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); - if (pause_countdown_enabled_) { - if (pause_countdown_ > 0) - --pause_countdown_; - else { + if (pause_period_start_ > 0) { + if (ClockFactory::get()->elapsedSince(pause_period_start_) >= pause_period_) { if (!isPaused()) pause(true); - pause_countdown_enabled_ = false; + pause_period_start_ = 0; } } diff --git a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h index c931b3cdf2..2ce4c3ee9c 100644 --- a/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Car/SimModeCar.h @@ -31,7 +31,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; private: void setupVehiclesAndCamera(std::vector& vehicles); @@ -44,6 +44,11 @@ class AIRSIM_API ASimModeCar : public ASimModeBase private: + typedef msr::airlib::ClockFactory ClockFactory; + typedef common_utils::Utils Utils; + typedef msr::airlib::TTimePoint TTimePoint; + typedef msr::airlib::TTimeDelta TTimeDelta; + UClass* external_camera_class_; UClass* camera_director_class_; @@ -53,7 +58,7 @@ class AIRSIM_API ASimModeCar : public ASimModeBase float follow_distance_; msr::airlib::StateReporterWrapper report_wrapper_; - float current_clockspeed_; - uint32_t pause_countdown_; - bool pause_countdown_enabled_; + std::atomic current_clockspeed_; + std::atomic pause_period_; + std::atomic pause_period_start_; }; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 0b1b461637..cde38fa5e7 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -131,11 +131,11 @@ void ASimModeBase::pause(bool is_paused) throw std::domain_error("Pause is not implemented by SimMode"); } -void ASimModeBase::continueForTicks(uint32_t ticks) +void ASimModeBase::continueForTime(double seconds) { //should be overriden by derived class - unused(ticks); - throw std::domain_error("continueForTicks is not implemented by SimMode"); + unused(seconds); + throw std::domain_error("continueForTime is not implemented by SimMode"); } std::unique_ptr ASimModeBase::createApiServer() const @@ -330,9 +330,9 @@ void ASimModeBase::SimModeApi::pause(bool is_paused) simmode_->pause(is_paused); } -void ASimModeBase::SimModeApi::continueForTicks(uint32_t ticks) +void ASimModeBase::SimModeApi::continueForTime(double seconds) { - simmode_->continueForTicks(ticks); + simmode_->continueForTime(seconds); } //************************* SimModeApi *****************************/ diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index e61c0f6bbe..123ddca578 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -55,7 +55,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual bool isPaused() const; virtual void pause(bool is_paused); - virtual void continueForTicks(uint32_t ticks); + virtual void continueForTime(double seconds); protected: typedef msr::airlib::AirSimSettings AirSimSettings; @@ -75,6 +75,8 @@ class AIRSIM_API ASimModeBase : public AActor typedef common_utils::Utils Utils; typedef msr::airlib::ClockFactory ClockFactory; typedef msr::airlib::TTimePoint TTimePoint; + typedef msr::airlib::TTimeDelta TTimeDelta; + class SimModeApi : public msr::airlib::SimModeApiBase { public: @@ -82,7 +84,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual msr::airlib::VehicleApiBase* getVehicleApi() override; virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; private: ASimModeBase* simmode_; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 8a3264553c..1115cb963f 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -97,9 +97,9 @@ void ASimModeWorldBase::pause(bool is_paused) physics_world_->pause(is_paused); } -void ASimModeWorldBase::continueForTicks(uint32_t ticks) +void ASimModeWorldBase::continueForTime(double seconds) { - physics_world_->continueForTicks(ticks); + physics_world_->continueForTime(seconds); } diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 8da23165a1..2f2daa2c0e 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -30,7 +30,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase virtual bool isPaused() const override; virtual void pause(bool is_paused) override; - virtual void continueForTicks(uint32_t ticks) override; + virtual void continueForTime(double seconds) override; protected: typedef std::shared_ptr VehiclePtr; diff --git a/docs/apis.md b/docs/apis.md index 49e41d0e59..ebd8284f44 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -93,14 +93,17 @@ You can find a ready to run project in HelloCar folder in the repository. * `simPrintLogMessage`: Prints the specified message in the simulator's window. If message_param is also supplied then its printed next to the message and in that case if this API is called with same message value but different message_param again then previous line is overwritten with new line (instead of API creating new line on display). For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API is called with different values of i. The valid values of severity parameter is 0 to 3 inclusive that corresponds to different colors. * `simGetObjectPose`: Gets the pose of specified object in Unreal environment. Here the object means "actor" in Unreal terminology. They are searched by tag as well as name. To add tag to actor, go to object in Unreal Editor, click on it, look for [Tags property](https://answers.unrealengine.com/questions/543807/whats-the-difference-between-tag-and-tag.html), click "+" sign and add some string value. If multiple actors have same tag then the first match is returned. If no matches are found then NaN pose is returned. -### Coordinate System -All AirSim API uses NED coordinate system, i.e., +X is North, +Y is East and +Z is Down. All units are in SI system. Please note that this is different from coordinate system used internally by Unreal Engine. In Unreal Engine, +Z is up instead of down and length unit is in centimeters instead of meters. AirSim APIs takes care of the appropriate conversions. The starting point of the vehicle is always coordinates (0, 0, 0) in NED system. Thus when converting from Unreal coordinates to NED, we first subtract the starting offset and then scale by 100 for cm to m conversion. - -## Image / Computer Vision and Collision APIs +### Image / Computer Vision and Collision APIs AirSim offers comprehensive images APIs to retrieve synchronized images from multiple cameras along with ground truth including depth, disparity, surface normals and vision. You can set the resolution, FOV, motion blur etc parameters in [settings.json](settings.md). There is also API for detecting collision state. See also [complete code](../Examples/StereoImageGenerator.hpp) that generates specified number of stereo images and ground truth depth with normalization to camera plan, computation of disparity image and saving it to [pfm format](pfm.md). More on [image APIs and Computer Vision mode](image_apis.md). +### Pause and Continue APIs +AirSim allows to pause and continue the simulation through `pause(is_paused)` API. To pause the simulation call `pause(True)` and to continue the simulation call `pause(False)`. You may have scenario, especially while runninf reinforcement learning, to run the simulation for specified amount of time and then automatically pause. While simulation is paused, you may then do some expensive computation, issue a new command and then again run the simulation for specified amount of time. This can be achieved by another API `continueForTime(seconds)`. This API runs the simulation for the specified number of seconds and then pauses the simulation. For example usage, please see [pause_continue_car.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_car.py) and [pause_continue_drone.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/pause_continue_drone.py). + +### Coordinate System +All AirSim API uses NED coordinate system, i.e., +X is North, +Y is East and +Z is Down. All units are in SI system. Please note that this is different from coordinate system used internally by Unreal Engine. In Unreal Engine, +Z is up instead of down and length unit is in centimeters instead of meters. AirSim APIs takes care of the appropriate conversions. The starting point of the vehicle is always coordinates (0, 0, 0) in NED system. Thus when converting from Unreal coordinates to NED, we first subtract the starting offset and then scale by 100 for cm to m conversion. + ## Vehicle Specific APIs ### APIs for Car Car has followings APIs available: