Simplify DroneControllerBase::getImageForCamera API and fix implement…

…ation so it

shares the same Unreal API that the recording thread was using (ReadSurfaceData).
This also makes it possible now to continually get multiple types of images (depth, segmentation, scene).

AirLib/AirLib.vcxproj

@@ -75,6 +75,7 @@
     <ClInclude Include="include\controllers\rosflight\firmware\turbotrig\turbotrig.h" />
     <ClInclude Include="include\controllers\rosflight\firmware\turbotrig\turbovec.h" />
     <ClInclude Include="include\controllers\rosflight\RosFlightDroneController.hpp" />
+    <ClInclude Include="include\controllers\VehicleCamera.hpp" />
     <ClInclude Include="include\rpc\ControlServerBase.hpp" />
     <ClInclude Include="include\safety\CubeGeoFence.hpp" />
     <ClInclude Include="include\controllers\DroneCommon.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -333,6 +333,9 @@
     <ClInclude Include="include\common\common_utils\WorkerThread.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\controllers\VehicleCamera.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp">

AirLib/include/common/common_utils/WorkerThread.hpp

@@ -85,15 +85,27 @@ namespace msr {
             void wait(_Predicate cancel)
             {
                 // wait for signal or cancel predicate
-                std::unique_lock<std::mutex> lock(mutex_);
                 while (!signaled_) {
+                    std::unique_lock<std::mutex> lock(mutex_);
                     cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
                         return cancel();
                     });
                 }
                 signaled_ = false;
             }
 
+            bool waitFor(double max_wait_seconds)
+            {
+                // wait for signal or timeout or cancel predicate
+                while (!signaled_) {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    cv_.wait_for(lock, std::chrono::milliseconds(
+                        static_cast<long long>(max_wait_seconds * 1000)));
+                }
+                signaled_ = false;
+                return true;
+            }
+
         };
 
         // This class provides a synchronized worker thread that guarantees to execute

AirLib/include/controllers/DroneControllerBase.hpp

@@ -15,6 +15,8 @@
 
 namespace msr { namespace airlib {
 
+    class VehicleCamera;
+
 /// DroneControllerBase represents a generic drone that can be controlled and queried for current state.
 /// All control methods return a boolean where true means the command was completed successfully, and
 /// false means the command was cancelled.  
@@ -222,20 +224,12 @@ class DroneControllerBase : public VehicleControllerBase {
         float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);
     virtual const VehicleParams& getVehicleParams() = 0;
 
-
-    /// Call this method when you want to start requesting certain types of images for a given camera.  Camera id's start at 0
-    /// and increment from there.  The number of cameras you can configure depends on the drone (or simulator).
-    virtual void setImageTypeForCamera(int camera_id, ImageType type);
-
-    /// Get the image type that is configured for the given camera id.
-    virtual ImageType getImageTypeForCamera(int camera_id);
-
-    /// After calling setImageTypeForCamera you can tghen request the actual images using this method.
-    /// The image is return in the .png format.  
+    /// Request an image of specific type from the specified camera.  Currently AirSim is configured with only 2 cameras which
+    /// have id of 0 and 1.  Camera 0 is setup to be an FPV camera view and camera 1 is setup as a 3rd person view that chases the drone.
+    /// The image is return in the .png format.  This call will block until the render is complete.  
     virtual vector<uint8_t> getImageForCamera(int camera_id, ImageType type);
 
-    /// bugbug: what is this doing here?  This should be a private implementation detail of the particular drone implementation.
-    virtual void setImageForCamera(int camera_id, ImageType type, const vector<uint8_t>& image);
+    virtual void addCamera(std::shared_ptr<VehicleCamera> camera);
 
     //*********************************common pre & post for move commands***************************************************
     //TODO: make these protected
@@ -263,8 +257,7 @@ class DroneControllerBase : public VehicleControllerBase {
     virtual float getDistanceAccuracy() = 0; 
 
     //naked variables for derived class access
-    unordered_map<int, ImageType> enabled_images;
-
+    unordered_map<int, std::shared_ptr<VehicleCamera>> enabled_cameras;
     struct EnumClassHash
     {
         template <typename T>

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -230,21 +230,7 @@ class DroneControllerCancelable  {
         controller_->getStatusMessages(messages);
     }
 
-
-    //request image
-    void setImageTypeForCamera(int camera_id, DroneControllerBase::ImageType type)
-    {
-        controller_->setImageTypeForCamera(camera_id, type);
-    }
-    DroneControllerBase::ImageType getImageTypeForCamera(int camera_id)
-    {
-        return controller_->getImageTypeForCamera(camera_id);
-    }
-    //get/set image
-    void setImageForCamera(int camera_id, DroneControllerBase::ImageType type, const vector<uint8_t>& image)
-    {
-        return controller_->setImageForCamera(camera_id, type, image);
-    }
+    //get image
     vector<uint8_t> getImageForCamera(int camera_id, DroneControllerBase::ImageType type)
     {
         return controller_->getImageForCamera(camera_id, type);

AirLib/include/controllers/MavLinkDroneController.hpp

@@ -133,7 +133,7 @@ class MavLinkDroneController : public DroneControllerBase
     const VehicleParams& getVehicleParams() override;
     //*** End: DroneControllerBase implementation ***//
 
-public: //pimpl
+private: //pimpl
     struct impl;
     std::unique_ptr<impl> pimpl_;
 };

AirLib/include/controllers/VehicleCamera.hpp

@@ -0,0 +1,22 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef air_VehicleCamera_hpp
+#define air_VehicleCamera_hpp
+
+#include "DroneControllerBase.hpp"
+
+namespace msr {
+    namespace airlib {
+
+        // This is an abstraction for cameras associated with a vehicle.  Each camera has a unique id.
+        class VehicleCamera
+        {
+        public:
+            virtual int getId() = 0;
+            virtual bool getScreenShot(DroneControllerBase::ImageType imageType, std::vector<uint8_t>& compressedPng) = 0;
+        };
+    }
+};
+
+#endif

AirLib/include/rpc/RpcLibClient.hpp

@@ -55,9 +55,6 @@ class RpcLibClient {
     std::string getDebugInfo();
 
     //request image
-    void setImageTypeForCamera(int camera_id, DroneControllerBase::ImageType type);
-    DroneControllerBase::ImageType getImageTypeForCamera(int camera_id);
-    //get/set image
     vector<uint8_t> getImageForCamera(int camera_id, DroneControllerBase::ImageType type);
 
     bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,

AirLib/src/controllers/DroneControllerBase.cpp

@@ -14,6 +14,7 @@
 #include <iostream>
 #include <fstream>
 #include "controllers/DroneControllerBase.hpp"
+#include "controllers/VehicleCamera.hpp"
 #include "common/common_utils/FileSystem.hpp"
 
 namespace msr { namespace airlib {
@@ -651,66 +652,23 @@ bool DroneControllerBase::isYawWithinMargin(float yaw_target, float margin)
     return std::abs(yaw_current - yaw_target) <= margin;
 }    
 
-void DroneControllerBase::setImageTypeForCamera(int camera_id, ImageType type)
+void DroneControllerBase::addCamera(std::shared_ptr<VehicleCamera> camera)
 {
     StatusLock lock(this);
-
-    if (type == ImageType::None)
-        enabled_images.erase(camera_id);
-    else
-        enabled_images[camera_id] = type;
-}
-
-DroneControllerBase::ImageType DroneControllerBase::getImageTypeForCamera(int camera_id)
-{
-    StatusLock lock(this);
-
-    auto it = enabled_images.find(camera_id);
-    if (it != enabled_images.end())
-        return it->second;
-    return ImageType::None;
-}
-
-void DroneControllerBase::setImageForCamera(int camera_id, ImageType type, const vector<uint8_t>& image)
-{
-    StatusLock lock(this);
-
-    //TODO: perf work
-    auto it = images.find(camera_id);
-    if (it != images.end())
-        (it->second)[type] = image;
-
-    auto new_list = EnumClassUnorderedMap<ImageType, vector<uint8_t>>();
-    new_list[type] = image;
-    images[camera_id] = new_list;
+    enabled_cameras[camera->getId()] = camera;
 }
 
 vector<uint8_t> DroneControllerBase::getImageForCamera(int camera_id, ImageType type)
 {
     StatusLock lock(this);
-
-    //TODO: bug: MSGPACK bombs out if vector if of 0 size
-    static vector<uint8_t> empty_vec(1);
-
-    vector<uint8_t> result;
+    vector<uint8_t> png;
 
     //TODO: perf work
-    auto it = images.find(camera_id);
-    if (it != images.end()) {
-        auto it2 = it->second.find(type);
-        if (it2 != it->second.end())
-            result = it2->second;
-        else
-            result = empty_vec;
-
-    } else
-        result = empty_vec;
-
-    if (result.size() == 0) {
-        result = empty_vec;
-    }
-
-    return result;
+    auto it = enabled_cameras.find(camera_id);
+    if (it != enabled_cameras.end()) {
+        it->second->getScreenShot(type, png);
+    } 
+    return png;
 }
 
 Pose DroneControllerBase::getDebugPose()