added normals, depthvis, make depth planner, configurable subwindows

AirLib/include/api/RpcLibAdapators.hpp

@@ -192,7 +192,7 @@ class RpcLibAdapators {
 
     struct ImageRequest {
         uint8_t camera_id;
-        msr::airlib::VehicleCameraBase::ImageType_ image_type;
+        msr::airlib::VehicleCameraBase::ImageType image_type;
         bool pixels_as_float;
         bool compress;
 
@@ -204,7 +204,7 @@ class RpcLibAdapators {
         ImageRequest(const msr::airlib::DroneControllerBase::ImageRequest& s)
         {
             camera_id = s.camera_id;
-            image_type = s.image_type.toEnum();
+            image_type = s.image_type;
             pixels_as_float = s.pixels_as_float;
             compress = s.compress;
         }
@@ -241,7 +241,9 @@ class RpcLibAdapators {
     };
 
     struct ImageResponse {
-        std::vector<uint8_t> image_data;
+        std::vector<uint8_t> image_data_uint8;
+        std::vector<float> image_data_float;
+
         Vector3r camera_position;
         Quaternionr camera_orientation;
         msr::airlib::TTimePoint time_stamp;
@@ -250,19 +252,28 @@ class RpcLibAdapators {
         bool compress;
         int width, height;
 
-        MSGPACK_DEFINE_ARRAY(image_data, camera_position, camera_orientation, time_stamp, message, pixels_as_float, compress, width, height);
+        MSGPACK_DEFINE_ARRAY(image_data_uint8, image_data_float, camera_position, camera_orientation, time_stamp, message, pixels_as_float, compress, width, height);
 
         ImageResponse()
         {}
 
         ImageResponse(const msr::airlib::VehicleCameraBase::ImageResponse& s)
         {
-            image_data = s.image_data;
+            pixels_as_float = s.pixels_as_float;
+
+            image_data_uint8 = s.image_data_uint8;
+            image_data_float = s.image_data_float;
+
+            //TODO: remove bug workaround for https://github.com/rpclib/rpclib/issues/152
+            if (image_data_uint8.size() == 0)
+                image_data_uint8.push_back(0);
+            if (image_data_float.size() == 0)
+                image_data_float.push_back(0);
+
             camera_position = Vector3r(s.camera_position);
             camera_orientation = Quaternionr(s.camera_orientation);
             time_stamp = s.time_stamp;
             message = s.message;
-            pixels_as_float = s.pixels_as_float;
             compress = s.compress;
             width = s.width;
             height = s.height;
@@ -272,12 +283,17 @@ class RpcLibAdapators {
         {
             msr::airlib::VehicleCameraBase::ImageResponse d;
 
-            d.image_data = image_data;
+            d.pixels_as_float = pixels_as_float;
+
+            if (! pixels_as_float)
+                d.image_data_uint8 = image_data_uint8;
+            else
+                d.image_data_float = image_data_float;
+
             d.camera_position = camera_position.to();
             d.camera_orientation = camera_orientation.to();
             d.time_stamp = time_stamp;
             d.message = message;
-            d.pixels_as_float = pixels_as_float;
             d.compress = compress;
             d.width = width;
             d.height = height;
@@ -311,7 +327,7 @@ class RpcLibAdapators {
 MSGPACK_ADD_ENUM(msr::airlib::DrivetrainType);
 MSGPACK_ADD_ENUM(msr::airlib::SafetyEval::SafetyViolationType_);
 MSGPACK_ADD_ENUM(msr::airlib::SafetyEval::ObsAvoidanceStrategy);
-MSGPACK_ADD_ENUM(msr::airlib::VehicleCameraBase::ImageType_);
+MSGPACK_ADD_ENUM(msr::airlib::VehicleCameraBase::ImageType);
 
 
 #endif

AirLib/include/common/common_utils/Utils.hpp

@@ -616,6 +616,51 @@ class Utils {
 #endif
     }
 
+    //convert strongly typed enum to underlying scaler types
+    template <typename E>
+    static constexpr typename std::underlying_type<E>::type toNumeric(E e) {
+        return static_cast<typename std::underlying_type<E>::type>(e);
+    }
+    template <typename E>
+    static constexpr E toEnum(typename std::underlying_type<E>::type u) {
+        return static_cast<E>(u);
+    }
+
+    // check whether machine is little endian
+    static bool isLittleEndian()
+    {
+        int intval = 1;
+        unsigned char *uval = (unsigned char *)&intval;
+        return uval[0] == 1;
+    }
+
+    static void writePfmFile(const float * const image_data, int width, int height, std::string path, float scalef=1)
+    {
+        std::fstream file(path.c_str(), std::ios::out | std::ios::binary);
+
+        std::string bands;
+        float fvalue;       // scale factor and temp value to hold pixel value
+        bands = "Pf";   // grayscale
+
+                        // sign of scalefact indicates endianness, see pfms specs
+        if(isLittleEndian())
+            scalef = -scalef;
+
+        // insert header information 
+        file << bands   << "\n";
+        file << width   << " ";
+        file << height  << "\n";
+        file << scalef  << "\n";
+
+        if(bands == "Pf"){          // handle 1-band image 
+            for (int i=0; i < height; i++) {
+                for(int j=0; j < width; ++j){
+                    fvalue = image_data[i * width + j];
+                    file.write((char*) &fvalue, sizeof(fvalue));
+                }
+            }
+        }
+    }
 };
 
 } //namespace

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -181,7 +181,7 @@ class DroneControllerCancelable  {
         vector<DroneControllerBase::ImageRequest> request = { DroneControllerBase::ImageRequest(camera_id, image_type)};
         const vector<VehicleCameraBase::ImageResponse>& response = simGetImages(request);
         if (response.size() > 0)
-            return response.at(0).image_data;
+            return response.at(0).image_data_uint8;
         else 
             return vector<uint8_t>();
     }

AirLib/include/controllers/Settings.hpp

@@ -108,13 +108,33 @@ class Settings {
 
     bool getChild(std::string name, Settings& child) const
     {
-        if (doc_.count(name) == 1 && doc_[name].type() == nlohmann::detail::value_t::object) {
+        if (doc_.count(name) == 1 && 
+            ( doc_[name].type() == nlohmann::detail::value_t::object ||
+                doc_[name].type() == nlohmann::detail::value_t::array
+            )) {
             child.doc_ = doc_[name].get<nlohmann::json>();
             return true;
         }
         return false;
     }
 
+    size_t size() {
+        return doc_.size();
+    }
+
+    bool getChild(size_t index, Settings& child) const
+    {
+        if (doc_.size() > index && 
+            ( doc_[index].type() == nlohmann::detail::value_t::object ||
+                doc_[index].type() == nlohmann::detail::value_t::array
+            )) {
+
+            child.doc_ = doc_[index].get<nlohmann::json>();
+            return true;
+        }
+        return false;
+    }
+
     std::string getString(std::string name, std::string defaultValue) const
     {
         if (doc_.count(name) == 1) {

AirLib/include/controllers/VehicleCameraBase.hpp

@@ -12,25 +12,27 @@ namespace msr { namespace airlib {
         class VehicleCameraBase
         {
         public: //types
-            enum class ImageType_ : uint {
-                None = 0,
-                Scene = 1, 
-                Depth = 2, 
-                Segmentation = 4,
-                Normals = 8,
-                All = 255
+            enum class ImageType : uint { //this indexes to array
+                Scene = 0, 
+                DepthMeters, 
+                DepthVis, 
+                DisparityNormalized,
+                Segmentation,
+                Normals,
+                Count //must be last
             };
-            typedef common_utils::EnumFlags<ImageType_>  ImageType;
 
             struct ImageResponse {
-                vector<uint8_t> image_data;
+                vector<uint8_t> image_data_uint8;
+                vector<float> image_data_float;
+
                 Vector3r camera_position = Vector3r::Zero();
                 Quaternionr camera_orientation = Quaternionr::Identity();
                 TTimePoint time_stamp = 0;
                 std::string message;
-                bool pixels_as_float;
-                bool compress;
-                int width, height;
+                bool pixels_as_float = false;
+                bool compress = true;
+                int width = 0, height = 0;
             };
 
         public: //methods

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightCommLink.hpp

@@ -38,7 +38,7 @@ class AirSimSimpleFlightCommLink : public simple_flight::ICommLink {
     virtual void log(const std::string& message, int32_t log_level = ICommLink::kLogLevelInfo)
     {
         if (log_level > 0)
-            throw std::runtime_error(message.c_str());
+            Utils::DebugBreak();
         messages_.push_back(std::string(message));
     }
 

AirLib/src/api/RpcLibClient.cpp

@@ -170,7 +170,7 @@ vector<VehicleCameraBase::ImageResponse> RpcLibClient::simGetImages(vector<Drone
 }
 vector<uint8_t> RpcLibClient::simGetImage(int camera_id, VehicleCameraBase::ImageType type)
 {
-    vector<uint8_t> result = pimpl_->client.call("simGetImage", camera_id, type.toEnum()).as<vector<uint8_t>>();
+    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();

AirLib/src/api/RpcLibServer.cpp

@@ -100,7 +100,7 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre
         const auto& response = drone_->simGetImages(RpcLibAdapators::ImageRequest::to(request_adapter)); 
         return RpcLibAdapators::ImageResponse::from(response);
     });
-    pimpl_->server.bind("simGetImage", [&](uint8_t camera_id, VehicleCameraBase::ImageType_ type) -> vector<uint8_t> { 
+    pimpl_->server.bind("simGetImage", [&](uint8_t camera_id, VehicleCameraBase::ImageType type) -> vector<uint8_t> { 
         auto result = drone_->simGetImage(camera_id, type); 
         if (result.size() == 0) {
             // rpclib has a bug with serializing empty vectors, so we return a 1 byte vector instead.

DroneShell/src/main.cpp

@@ -1173,19 +1173,23 @@ class GetImageCommand : public DroneCommand {
             }
 
             const char* typeName = "";
-            switch (imageType.toEnum())
+            switch (imageType)
             {
-            case msr::airlib::VehicleCameraBase::ImageType_::Scene:
+            case msr::airlib::VehicleCameraBase::ImageType::Scene:
                 typeName = "scene";
                 break;
-            case msr::airlib::VehicleCameraBase::ImageType_::Depth:
+            case msr::airlib::VehicleCameraBase::ImageType::DepthVis:
                 typeName = "depth";
                 break;
-            case msr::airlib::VehicleCameraBase::ImageType_::Segmentation:
+            case msr::airlib::VehicleCameraBase::ImageType::Segmentation:
                 typeName = "seg";
                 break;
-            case msr::airlib::VehicleCameraBase::ImageType_::None:
-            case msr::airlib::VehicleCameraBase::ImageType_::All:
+            case msr::airlib::VehicleCameraBase::ImageType::Normals:
+                typeName = "normals";
+                break;
+            case msr::airlib::VehicleCameraBase::ImageType::DisparityNormalized:
+                typeName = "disparity";
+                break;
             default:
                 break;
             }
@@ -1216,11 +1220,15 @@ class GetImageCommand : public DroneCommand {
         VehicleCameraBase::ImageType imageType;
 
         if (type == "depth") {
-            imageType = VehicleCameraBase::ImageType_::Depth;
+            imageType = VehicleCameraBase::ImageType::DepthVis;
         } else if (type == "scene") {
-            imageType = VehicleCameraBase::ImageType_::Scene;
+            imageType = VehicleCameraBase::ImageType::Scene;
         } else if (type == "segmentation") {
-            imageType = VehicleCameraBase::ImageType_::Segmentation;
+            imageType = VehicleCameraBase::ImageType::Segmentation;
+        } else if (type == "normals") {
+            imageType = VehicleCameraBase::ImageType::Normals;
+        } else if (type == "disparity") {
+            imageType = VehicleCameraBase::ImageType::DisparityNormalized;
         } else {
             cout << "Error: Invalid image type '" << type << "', expecting either 'depth', 'scene' or 'segmentation'" << endl;
             return true;