pause-continue pattern

zwbai · Apr 24, 2018 · 953cb6c · 953cb6c
1 parent 2e1b666
commit 953cb6c
Show file tree

Hide file tree

Showing 14 changed files with 290 additions and 110 deletions.
diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp
@@ -43,6 +43,10 @@ class RpcLibClientBase {
     CameraInfo getCameraInfo(int camera_id);
     void setCameraOrientation(int camera_id, const Quaternionr& orientation);
 
+    bool simIsPaused();
+    void simPause(bool is_paused);
+    void simContinueForTicks(uint32_t ticks);
+
     virtual ~RpcLibClientBase();    //required for pimpl
 
 protected:

diff --git a/AirLib/include/api/SimModeApiBase.hpp b/AirLib/include/api/SimModeApiBase.hpp
@@ -13,6 +13,9 @@ namespace msr { namespace airlib {
 class SimModeApiBase {
 public:
     virtual VehicleApiBase* getVehicleApi() = 0;
+    virtual bool isPaused() const = 0;
+    virtual void pause(bool is_paused) = 0;
+    virtual void continueForTicks(uint32_t ticks) = 0;
     virtual ~SimModeApiBase() = default;
 };
 

diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp
@@ -85,39 +85,6 @@ bool RpcLibClientBase::isApiControlEnabled()
     return pimpl_->client.call("isApiControlEnabled").as<bool>();
 }
 
-//sim only
-void RpcLibClientBase::simSetPose(const Pose& pose, bool ignore_collision)
-{
-    pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision);
-}
-Pose RpcLibClientBase::simGetPose()
-{
-    return pimpl_->client.call("simGetPose").as<RpcLibAdapatorsBase::Pose>().to();
-}
-vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<ImageCaptureBase::ImageRequest> request)
-{
-    const auto& response_adaptor = pimpl_->client.call("simGetImages", 
-        RpcLibAdapatorsBase::ImageRequest::from(request))
-        .as<vector<RpcLibAdapatorsBase::ImageResponse>>();
-
-    return RpcLibAdapatorsBase::ImageResponse::to(response_adaptor);
-}
-vector<uint8_t> RpcLibClientBase::simGetImage(int camera_id, ImageCaptureBase::ImageType type)
-{
-    vector<uint8_t> result = pimpl_->client.call("simGetImage", camera_id, type).as<vector<uint8_t>>();
-    if (result.size() == 1) {
-        // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead.
-        result.clear();
-    }
-    return result;
-}
-
-void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char  severity)
-{
-    pimpl_->client.call("simPrintLogMessage", message, message_param, severity);
-}
-
-
 msr::airlib::GeoPoint RpcLibClientBase::getHomeGeoPoint()
 {
     return pimpl_->client.call("getHomeGeoPoint").as<RpcLibAdapatorsBase::GeoPoint>().to();
@@ -167,6 +134,55 @@ CollisionInfo RpcLibClientBase::getCollisionInfo()
     return pimpl_->client.call("getCollisionInfo").as<RpcLibAdapatorsBase::CollisionInfo>().to();
 }
 
+
+//sim only
+void RpcLibClientBase::simSetPose(const Pose& pose, bool ignore_collision)
+{
+    pimpl_->client.call("simSetPose", RpcLibAdapatorsBase::Pose(pose), ignore_collision);
+}
+Pose RpcLibClientBase::simGetPose()
+{
+    return pimpl_->client.call("simGetPose").as<RpcLibAdapatorsBase::Pose>().to();
+}
+vector<ImageCaptureBase::ImageResponse> RpcLibClientBase::simGetImages(vector<ImageCaptureBase::ImageRequest> request)
+{
+    const auto& response_adaptor = pimpl_->client.call("simGetImages", 
+        RpcLibAdapatorsBase::ImageRequest::from(request))
+        .as<vector<RpcLibAdapatorsBase::ImageResponse>>();
+
+    return RpcLibAdapatorsBase::ImageResponse::to(response_adaptor);
+}
+vector<uint8_t> RpcLibClientBase::simGetImage(int camera_id, ImageCaptureBase::ImageType type)
+{
+    vector<uint8_t> result = pimpl_->client.call("simGetImage", camera_id, type).as<vector<uint8_t>>();
+    if (result.size() == 1) {
+        // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead.
+        result.clear();
+    }
+    return result;
+}
+
+void RpcLibClientBase::simPrintLogMessage(const std::string& message, std::string message_param, unsigned char  severity)
+{
+    pimpl_->client.call("simPrintLogMessage", message, message_param, severity);
+}
+
+
+bool RpcLibClientBase::simIsPaused()
+{
+    return pimpl_->client.call("simIsPaused").as<bool>();
+}
+
+void RpcLibClientBase::simPause(bool is_paused)
+{
+    pimpl_->client.call("simPause", is_paused);
+}
+
+void RpcLibClientBase::simContinueForTicks(uint32_t ticks)
+{
+    pimpl_->client.call("simContinueForTicks", ticks);
+}
+
 msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name)
 {
     return pimpl_->client.call("simGetObjectPose", object_name).as<RpcLibAdapatorsBase::Pose>().to();

diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp
@@ -122,6 +122,16 @@ RpcLibServerBase::RpcLibServerBase(SimModeApiBase* simmode_api, string server_ad
         return RpcLibAdapatorsBase::Pose(pose);
     });
 
+    pimpl_->server.bind("simPause", [&](bool is_paused) -> void { 
+        getSimModeApi()->pause(is_paused); 
+    });
+    pimpl_->server.bind("simIsPaused", [&]() -> bool { 
+        return getSimModeApi()->isPaused(); 
+    });
+    pimpl_->server.bind("simContinueForTicks", [&](uint32_t ticks) -> void { 
+        getSimModeApi()->continueForTicks(ticks); 
+    });
+
     pimpl_->server.suppress_exceptions(true);
 }
 

diff --git a/PythonClient/AirSimClient.py b/PythonClient/AirSimClient.py
@@ -213,67 +213,6 @@ class AirSimClientBase:
     def __init__(self, ip, port):
         self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = 3600, pack_encoding = 'utf-8', unpack_encoding = 'utf-8')
 
-    def ping(self):
-        return self.client.call('ping')
-
-    def reset(self):
-        self.client.call('reset')
-
-    def confirmConnection(self):
-        home = self.getHomeGeoPoint()
-        while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or
-                math.isnan(home.latitude) or  math.isnan(home.longitude) or  math.isnan(home.altitude)):
-            time.sleep(1)
-            home = self.getHomeGeoPoint()
-            print('X', end='')
-        print('')
-
-    def getHomeGeoPoint(self):
-        return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint'))
-
-    # basic flight control
-    def enableApiControl(self, is_enabled):
-        return self.client.call('enableApiControl', is_enabled)
-    def isApiControlEnabled(self):
-        return self.client.call('isApiControlEnabled')
-
-    def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False):
-        return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex)
-    def simGetSegmentationObjectID(self, mesh_name):
-        return self.client.call('simGetSegmentationObjectID', mesh_name)
-    def simPrintLogMessage(self, message, message_param = "", severity = 0):
-        return self.client.call('simPrintLogMessage', message, message_param, severity)
-    def simGetObjectPose(self, object_name):
-        pose = self.client.call('simGetObjectPose', object_name)
-        return Pose.from_msgpack(pose)
-
-
-    # camera control
-    # simGetImage returns compressed png in array of bytes
-    # image_type uses one of the AirSimImageType members
-    def simGetImage(self, camera_id, image_type):
-        # because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
-        result = self.client.call('simGetImage', camera_id, image_type)
-        if (result == "" or result == "\0"):
-            return None
-        return result
-
-    # camera control
-    # simGetImage returns compressed png in array of bytes
-    # image_type uses one of the AirSimImageType members
-    def simGetImages(self, requests):
-        responses_raw = self.client.call('simGetImages', requests)
-        return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw]
-
-    def getCollisionInfo(self):
-        return CollisionInfo.from_msgpack(self.client.call('getCollisionInfo'))
-
-    def getCameraInfo(self, camera_id):
-        return CameraInfo.from_msgpack(self.client.call('getCameraInfo', camera_id))
-
-    def setCameraOrientation(self, camera_id, orientation):
-        self.client.call('setCameraOrientation', camera_id, orientation)
-
     @staticmethod
     def stringToUint8Array(bstr):
         return np.fromstring(bstr, np.uint8)
@@ -496,6 +435,73 @@ def png_pack(png_tag, data):
 
         AirSimClientBase.write_file(filename, png_bytes)
 
+    def ping(self):
+        return self.client.call('ping')
+
+    def reset(self):
+        self.client.call('reset')
+
+    def confirmConnection(self):
+        home = self.getHomeGeoPoint()
+        while ((home.latitude == 0 and home.longitude == 0 and home.altitude == 0) or
+                math.isnan(home.latitude) or  math.isnan(home.longitude) or  math.isnan(home.altitude)):
+            time.sleep(1)
+            home = self.getHomeGeoPoint()
+            print('X', end='')
+        print('')
+
+    def getHomeGeoPoint(self):
+        return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint'))
+
+    # basic flight control
+    def enableApiControl(self, is_enabled):
+        return self.client.call('enableApiControl', is_enabled)
+    def isApiControlEnabled(self):
+        return self.client.call('isApiControlEnabled')
+
+    def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False):
+        return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex)
+    def simGetSegmentationObjectID(self, mesh_name):
+        return self.client.call('simGetSegmentationObjectID', mesh_name)
+    def simPrintLogMessage(self, message, message_param = "", severity = 0):
+        return self.client.call('simPrintLogMessage', message, message_param, severity)
+    def simGetObjectPose(self, object_name):
+        pose = self.client.call('simGetObjectPose', object_name)
+        return Pose.from_msgpack(pose)
+
+
+    # camera control
+    # simGetImage returns compressed png in array of bytes
+    # image_type uses one of the AirSimImageType members
+    def simGetImage(self, camera_id, image_type):
+        # because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
+        result = self.client.call('simGetImage', camera_id, image_type)
+        if (result == "" or result == "\0"):
+            return None
+        return result
+
+    # camera control
+    # simGetImage returns compressed png in array of bytes
+    # image_type uses one of the AirSimImageType members
+    def simGetImages(self, requests):
+        responses_raw = self.client.call('simGetImages', requests)
+        return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw]
+
+    def getCollisionInfo(self):
+        return CollisionInfo.from_msgpack(self.client.call('getCollisionInfo'))
+
+    def getCameraInfo(self, camera_id):
+        return CameraInfo.from_msgpack(self.client.call('getCameraInfo', camera_id))
+
+    def setCameraOrientation(self, camera_id, orientation):
+        self.client.call('setCameraOrientation', camera_id, orientation)
+
+    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))
 
 # -----------------------------------  Multirotor APIs ---------------------------------------------
 class MultirotorClient(AirSimClientBase, object):

diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj
@@ -5,7 +5,7 @@
     <SchemaVersion>2.0</SchemaVersion>
     <ProjectGuid>e2049e20-b6dd-474e-8bca-1c8dc54725aa</ProjectGuid>
     <ProjectHome>.</ProjectHome>
-    <StartupFile>clock_speed.py</StartupFile>
+    <StartupFile>pause_continue_car.py</StartupFile>
     <SearchPath>
     </SearchPath>
     <WorkingDirectory>.</WorkingDirectory>
@@ -35,6 +35,9 @@
     <Compile Include="car_collision.py">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="pause_continue_car.py">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="clock_speed.py">
       <SubType>Code</SubType>
     </Compile>
@@ -56,6 +59,9 @@
       <SubType>Code</SubType>
     </Compile>
     <Compile Include="orbit.py" />
+    <Compile Include="pause_continue_drone.py">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="point_cloud.py" />
     <Compile Include="seg_pallete.py">
       <SubType>Code</SubType>

diff --git a/PythonClient/pause_continue_car.py b/PythonClient/pause_continue_car.py
@@ -0,0 +1,29 @@
+from AirSimClient import *
+
+# connect to the AirSim simulator
+client = CarClient()
+client.confirmConnection()
+client.enableApiControl(True)
+
+car_controls = CarControls()
+
+for i in range(1, 6):
+    print("Starting command")
+    car_controls.throttle = 0.5
+    car_controls.steering = 1
+    client.setCarControls(car_controls)
+    time.sleep(5) #run
+    print("Pausing after 5sec")
+    client.simPause(True)
+    time.sleep(5) #paused
+    print("Restarting command to run for 10sec")
+    client.simContinueForTicks(600) #600 ticks of 1/60sec each = 10sec
+    time.sleep(20)
+    print("Finishing rest of the command")
+    client.simPause(False)
+    time.sleep(10)
+    print("Finished cycle")
+
+
+
+
diff --git a/PythonClient/pause_continue_drone.py b/PythonClient/pause_continue_drone.py
@@ -0,0 +1,30 @@
+from AirSimClient import *
+
+# connect to the AirSim simulator
+client = MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+client.armDisarm(True)
+
+print("Taking off")
+client.moveByVelocityZ(0, 0, -20, 8)
+time.sleep(3)    
+
+for i in range(1, 6):
+    print("Starting command to run for 15sec")
+    client.moveByVelocityZ(-1*i, -1*i, -20-i, 15)
+    time.sleep(5) #run
+    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
+    time.sleep(10)
+    print("Finishing rest of the command")
+    client.simPause(False)
+    time.sleep(10)
+    print("Finished cycle")
+
+
+
+
diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp
@@ -185,6 +185,11 @@ void UAirBlueprintLib::LogMessage(const FString &prefix, const FString &suffix,
     //GEngine->AddOnScreenDebugMessage(key + 10, 60.0f, color, FString::FromInt(key));
 }
 
+void UAirBlueprintLib::setUnrealClockSpeed(const AActor* context, float clock_speed)
+{
+    context->GetWorldSettings()->SetTimeDilation(clock_speed);
+}
+
 float UAirBlueprintLib::GetWorldToMetersScale(const AActor* context)
 {
     float w2m = 100.f;

diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h
@@ -111,6 +111,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary
     static UObject* LoadObject(const std::string& name);
     static UClass* LoadClass(const std::string& name);
 
+    static void setUnrealClockSpeed(const AActor* context, float clock_speed);
+
 private:
     template<typename T>
     static void InitializeObjectStencilID(T* obj, bool ignore_existing = true);