Fix C++ client request wedge (uninitialized flag) and depth packing (raw fp16 meters, 16FC1)

client/cpp/ProjectAirsimClientLib/src/AsyncResultInternal.h

@@ -34,7 +34,11 @@ template <typename TAncestor>
 class TAsyncResultProviderBase : public TRefCounted<TAncestor> {
  public:
   TAsyncResultProviderBase(void)
-      : cv_done_(), fis_done_(false), mutex_(), status_(Status::InProgress) {}
+      : cv_done_(),
+        fis_canceled_(false),
+        fis_done_(false),
+        mutex_(),
+        status_(Status::InProgress) {}
 
   // Returns whether the task has been requested to cancel the operation
   bool FIsCanceled(void) const { return (fis_canceled_); }

client/python/airsimv1_scripts_migrated/computer_vision/cv_capture.py

@@ -57,7 +57,7 @@ def camera_callback(camera_info, camera_name):
         responses.update(drone.get_images(camera_id="front_left", image_type_ids=[ImageType.SCENE]))
 
         for i, response in enumerate(responses.values()):
-            if response["encoding"] == "16UC1":
+            if response["encoding"] in ("16UC1", "16FC1"):
                 projectairsim_log().info("Type %s, size %d, pos %s" % (response["encoding"], len(response["data"]), pprint.pformat([response["pos_x"],response["pos_y"],response["pos_z"]])))
                 filename = os.path.normpath(os.path.join(tmp_dir, str(x) + "_" + str(i) + '.pfm'))
             else:

client/python/airsimv1_scripts_migrated/computer_vision/cv_mode.py

@@ -66,7 +66,7 @@ def to_quaternion(pitch, roll, yaw):
         #responses.update(drone.GetImages("back_center", [ImageType.DISPARITY_NORMALIZED, ImageType.SURFACE_NORMALS]))
 
         for idx, response in enumerate(responses.values()):
-            if response["encoding"] == "16UC1":
+            if response["encoding"] in ("16UC1", "16FC1"):
                 filename = os.path.join(tmp_dir, str(x) + "_" + str(idx) + ".pfm")
             else:
                 filename = os.path.join(tmp_dir, str(x) + "_" + str(idx) + ".png")

client/python/airsimv1_scripts_migrated/multirotor/hello_drone.py

@@ -92,7 +92,7 @@ async def main():
 
         for idx, image in enumerate(images.values()):
             img_np = unpack_image(image)
-            if image["encoding"] == "16UC1":
+            if image["encoding"] in ("16UC1", "16FC1"):
                 file_save_path = os.path.join(tmp_dir, str(idx) + ".pfm")
             else:
                 file_save_path = os.path.join(tmp_dir, str(idx) + ".png")

client/python/airsimv1_scripts_migrated/multirotor/multi_agent_drone.py

@@ -66,7 +66,7 @@ async def main():
 
         responses = [v for d in (responses1, responses2) for v in d.values()]
         for idx, response in enumerate(responses):
-            if response["encoding"] == "16UC1":
+            if response["encoding"] in ("16UC1", "16FC1"):
                 filename = os.path.join(tmp_dir, str(idx) + ".pfm")
             else:
                 filename = os.path.join(tmp_dir, str(idx) + ".png")

client/python/example_user_scripts/camera_image_types.py

@@ -67,7 +67,7 @@ async def main():
 
         for index, image in enumerate(images.values()):
             img_np = unpack_image(image)
-            if image["encoding"] == "16UC1":
+            if image["encoding"] in ("16UC1", "16FC1"):
                 file_save_path = os.path.join(save_path, str(index) + ".pfm")
             else:
                 file_save_path = os.path.join(save_path, str(index) + ".png")

client/python/projectairsim/src/projectairsim/utils.py

@@ -417,8 +417,14 @@ def unpack_image(image):
     Returns:
         The image in openCV decoded form
     """
-    # 16UC1 is used for serializing depth images
-    if image["encoding"] == "16UC1":
+    # 16FC1 is used for serializing depth images: raw IEEE 754 half-precision
+    # (binary16) METERS, little-endian, bit-exact with the sim's fp16 render
+    # target. Sky / no-hit pixels arrive as +inf.
+    if image["encoding"] == "16FC1":
+        img_dtype = "float16"
+        img_shape = [image["height"], image["width"]]
+    # 16UC1: legacy uint16 depth from older sims
+    elif image["encoding"] == "16UC1":
         img_dtype = "uint16"
         img_shape = [image["height"], image["width"]]
     elif image["encoding"] == "AVX":

ros/node/projectairsim-rosbridge/src/projectairsim_rosbridge/msg_converter.py

@@ -160,9 +160,13 @@ def convert_image_to_ros(self, projectairsim_topic_name, projectairsim_image):
             return self.convert_image_16uc1_to_ros(
                 projectairsim_topic_name, projectairsim_image
             )
+        elif projectairsim_image["encoding"] == "16FC1":
+            return self.convert_image_16fc1_to_ros(
+                projectairsim_topic_name, projectairsim_image
+            )
         else:
             raise ValueError(
-                f"Can only handle image encoding BGR or 16UC1, not \"{projectairsim_image['encoding']}\""
+                f"Can only handle image encoding BGR, 16UC1 or 16FC1, not \"{projectairsim_image['encoding']}\""
             )
 
     def convert_image_bgr8_to_ros(
@@ -229,6 +233,42 @@ def convert_image_16uc1_to_ros(
 
         return image
 
+    def convert_image_16fc1_to_ros(
+        self, projectairsim_topic_name, projectairsim_image_16fc1
+    ):
+        """
+        Convert a Project AirSim 16FC1 image message into a ROS image message.
+
+        16FC1 is raw IEEE 754 half-precision (binary16) depth in METERS,
+        little-endian, bit-exact with the sim's fp16 render target. Decoded to
+        the standard ROS 32FC1 float-meters depth image; non-finite pixels
+        (sky / no hit arrive as +inf) become NaN per the ROS depth convention.
+
+        Arguments:
+            projectairsim_topic_name - The Project AirSim topic name
+            projectairsim_image_16fc1 - The 16FC1 image message received from the Project AirSim topic
+
+        Return:
+            (return) - Corresponding ROS Image message
+        """
+        image = rossensmsg.Image()
+        image.header.stamp = self.ros_node.get_time_now_msg()
+        # image.header.frame_id must be set by caller
+
+        image.height = projectairsim_image_16fc1["height"]
+        image.width = projectairsim_image_16fc1["width"]
+        image.encoding = "32FC1"
+        image.is_bigendian = projectairsim_image_16fc1["big_endian"]
+
+        nparray = np.frombuffer(
+            projectairsim_image_16fc1["data"], dtype=np.float16
+        ).astype(np.float32)
+        nparray[~np.isfinite(nparray)] = np.nan
+        image.data = nparray.tobytes()
+        image.step = image.width * 4
+
+        return image
+
     def convert_imu_to_ros(self, projectairsim_topic_name, projectairsim_msg):
         """
         Convert a Project AirSim IMU sensor message into a ROS Imu message.

ros/projectairsim_ros2_cpp/include/projectairsim_ros2_cpp/ros2_conversion_utils.hpp

@@ -4,6 +4,7 @@
 #include <array>
 #include <cmath>
 #include <cstdint>
+#include <cstring>
 #include <filesystem>
 #include <limits>
 #include <string>
@@ -342,6 +343,35 @@ inline void PopulateCameraInfoFromJson(const json& msg,
   FillFixedArray(msg.value("projection_matrix", json::array()), &camera_info->p);
 }
 
+// Convert an IEEE 754 half-precision (binary16) bit pattern to float32.
+inline float HalfBitsToFloat(uint16_t half_bits) {
+  const uint32_t sign = static_cast<uint32_t>(half_bits & 0x8000u) << 16;
+  uint32_t exponent = (half_bits >> 10) & 0x1Fu;
+  uint32_t mantissa = half_bits & 0x3FFu;
+  uint32_t float_bits;
+  if (exponent == 0) {
+    if (mantissa == 0) {
+      float_bits = sign;  // +/- zero
+    } else {
+      // Subnormal half: normalize into a float32 exponent/mantissa.
+      exponent = 127 - 15 + 1;
+      while ((mantissa & 0x400u) == 0) {
+        mantissa <<= 1;
+        --exponent;
+      }
+      mantissa &= 0x3FFu;
+      float_bits = sign | (exponent << 23) | (mantissa << 13);
+    }
+  } else if (exponent == 31) {
+    float_bits = sign | 0x7F800000u | (mantissa << 13);  // inf / NaN
+  } else {
+    float_bits = sign | ((exponent - 15 + 127) << 23) | (mantissa << 13);
+  }
+  float result;
+  std::memcpy(&result, &float_bits, sizeof(result));
+  return result;
+}
+
 inline bool PopulateImagePayloadFromJson(const json& msg,
                                          sensor_msgs::msg::Image* image) {
   image->height = static_cast<uint32_t>(NumberOr(msg, "height"));
@@ -355,11 +385,34 @@ inline bool PopulateImagePayloadFromJson(const json& msg,
     return true;
   }
   if (encoding == "16UC1") {
+    // Legacy sims: uint16 depth. Kept for compatibility with older servers.
     image->encoding = "mono16";
     image->data = BytesFromJsonString(msg.value("data", ""));
     image->step = 2 * image->width;
     return true;
   }
+  if (encoding == "16FC1") {
+    // Depth as raw IEEE 754 half-precision (binary16) METERS, little-endian,
+    // bit-exact with the sim's fp16 render target. Decode to the standard
+    // ROS 32FC1 float-meters depth image; non-finite pixels (sky / no hit
+    // arrive as +inf) become NaN per the ROS depth convention.
+    const auto payload = BytesFromJsonString(msg.value("data", ""));
+    const size_t pixel_count = payload.size() / 2;
+    image->encoding = "32FC1";
+    image->step = 4 * image->width;
+    image->data.resize(pixel_count * 4);
+    const uint8_t* src = payload.data();
+    float* dst = reinterpret_cast<float*>(image->data.data());
+    for (size_t i = 0; i < pixel_count; ++i, src += 2) {
+      const uint16_t bits = static_cast<uint16_t>(src[0]) |
+                            (static_cast<uint16_t>(src[1]) << 8);
+      const float meters = HalfBitsToFloat(bits);
+      dst[i] = std::isfinite(meters)
+                   ? meters
+                   : std::numeric_limits<float>::quiet_NaN();
+    }
+    return true;
+  }
   return false;
 }
 

unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Sensors/ImagePackingAsyncTask.cpp

@@ -28,21 +28,26 @@ void FImagePackingAsyncTask::DoWork() {
 
     // Handle Depth image requests here.
     // 1. Currently, we do not support Compression for depth images
-    // 2. We also do not support sending back Floats since our ImageResponse
-    // Message is limited to uint8 for now. Instead, we convert Float16 from
-    // Unreal to uint16 and then pack it in two uint8s that have to be unpacked
-    // properly on client side. NOTE: This case also handles PixelsAsFloat
-    // implicitly i.e. it ignores it and always sends back uint16 for depth
+    // 2. Depth is transmitted as the render target's IEEE 754 half-precision
+    // (binary16) METERS, bit-exact: each pixel's FFloat16 bit pattern is
+    // packed little-endian into two uint8s (encoding "16FC1" below) and
+    // reinterpreted as float16 on the client side. No value conversion
+    // happens here, so no precision is lost beyond the fp16 render target
+    // itself, and there is no range cap: sky / no-hit pixels arrive as +inf
+    // for clients to map to their own invalid-depth convention. NOTE: This
+    // case also handles PixelsAsFloat implicitly i.e. it ignores it and
+    // always sends back float16 for depth
     if (bIsDepthImage && !ImageRequest.bCompress) {
       ImgResponse.ImageDataUInt8.resize(
           RenderResult.Width * RenderResult.Height * 2 * sizeof(uint8));
 
       uint8* DstPtr = ImgResponse.ImageDataUInt8.data();
       for (const auto& SrcPixel : RenderResult.UnrealImageFloat) {
-        float DepthMilli = SrcPixel.R.GetFloat(); 
-        uint16 DepthUint16 = static_cast<uint16>(DepthMilli);  
-        *DstPtr++ = static_cast<uint8>(DepthUint16 & 0xFF);        // least significant byte
-        *DstPtr++ = static_cast<uint8>((DepthUint16 >> 8) & 0xFF);  // most significant byte
+        // The depth materials write METERS to R; transmit the fp16 bit
+        // pattern as-is (see the encoding comment above).
+        const uint16 DepthHalfBits = SrcPixel.R.Encoded;
+        *DstPtr++ = static_cast<uint8>(DepthHalfBits & 0xFF);        // least significant byte
+        *DstPtr++ = static_cast<uint8>((DepthHalfBits >> 8) & 0xFF);  // most significant byte
       }
     }
     // Normal RGB images without compression or PixelsAsFloat requested
@@ -128,8 +133,9 @@ void FImagePackingAsyncTask::DoWork() {
         ImgEncoding = "BGR";
       }
     } else {  // bIsDepthImage
-      // 16-bit unsigned, 1 channel for depth in mm
-      ImgEncoding = "16UC1";
+      // IEEE 754 half-precision (binary16), 1 channel, depth in METERS,
+      // little-endian — bit-exact with the fp16 render target
+      ImgEncoding = "16FC1";
     }
 
     ImageMessages.emplace(