Fix C++ client request wedge (uninitialized flag), depth packed as meters, and wall-clock ROS stamps

[andrewjong]

About

Three fixes found while integrating the ROS2 C++ bridge into a mapping pipeline on Linux (Blocks, UE 5.7.4):

1. cpp client: initialize fis_canceled_ — TAsyncResultProviderBase's constructor initializer list skips fis_canceled_, leaving it uninitialized memory. Both client worker threads consult FIsCanceled(): when the garbage reads true, RequestSendingThreadProc silently skips sending the request and ResponseReceivingThreadProc pops the pending-response entry without ever calling SetDone, so the caller's Wait() blocks forever — and every later request queues behind it. In practice this permanently wedges the C++ client's whole request channel (the ROS2 bridge's /clock never publishes, move_*/SetPose service calls hang), nondeterministically depending on heap state — which is why it can appear to work on one run and deadlock on the next. One-line fix.

2. unreal: pack depth as actual millimeters with saturation — the depth materials write meters to the R channel, but FImagePackingAsyncTask casts that float straight to uint16 while the wire encoding is declared as 16UC1 depth in mm. Consumers therefore receive depth quantized to whole meters (a camera 2 m above ground reads raw 2). Sky / no-hit pixels (huge or inf) also wrap around in the bare cast into phantom finite depths. Fixed by converting to millimeters and saturating at 65535, which downstream can treat as "no return".

3. ros2 cpp bridge: stamp headers/TF with sim time — every Project AirSim sensor/pose message carries the sim clock in its time_stamp field, but the bridge stamped ROS headers with its own wall clock at receive time. That breaks time-synchronization downstream (an RGB/depth pair rendered on the same sim tick gets two different stamps, offset by network jitter) and makes header stamps inconsistent with the /clock topic the bridge itself publishes. The bridge now prefers the message's time_stamp when present, falling back to wall clock; TF broadcasts reuse the owning message's stamp.

How Has This Been Tested?

Linux (Ubuntu 22.04), Blocks built from this repo at current main with UE 5.7.4, ROS 2 Humble; the bridge node driving a camera+non-physics robot scene and a scene_drone_sensors.jsonc scene.

• Fix 1: Before — the bridge's /clock never published and all RawRequest/SetPose service calls hung indefinitely (reproduced on repeated runs and two different Blocks builds); a gdb thread dump showed the rclcpp executor blocked in Client::Request → AsyncResult::Wait() with both client worker threads idle and empty queues, i.e. the entry was consumed without completion via the FIsCanceled() path. The Python client against the same sim worked, which localized the fault to the C++ client. After — /clock publishes at a steady 50 Hz (the node's 20 ms poll), SetPose round-trips return success=True, and a downstream RGB-D mapping pipeline ran for extended sessions with no request wedges.
• Fix 2: Before — raw mono16 payloads read median 3 for a camera ~3 m from the scene (integer meters). After — median 3236 (mm), and pointing the camera 45° down from 2 m altitude reads the expected ~2.8 m slant range with millimeter-scale variation; sky pixels arrive as 65535 instead of wrapped small values.
• Fix 3: RGB and depth images from the same sim tick now carry identical header stamps (verified with a message_filters exact pairing downstream), consistent with /clock.

Screenshots and videos (if appropriate):

N/A — behavioral fixes; measurable evidence described above.

🤖 Generated with Claude Code

[jonyMarino]

We must indicate that the output is in meters, not millimeters. Multiplying by 1000 is unnecessary and adds error.

[andrewjong]

Thanks! Hm this is what Claude Fable said, thoughts?

I'd like to push back gently on this one. The R channel arrives as fp16 meters, so its precision floor is already ~4 mm at 4–8 m and ~31 mm at 32–64 m — the ×1000 multiply happens in float32 and adds at most 1 ulp on top of that, while the uint16-mm representation quantizes at 1 mm, i.e. below the fp16 floor. Keeping raw meters in a uint16 instead quantizes depth to whole meters (before this patch, a camera 3.24 m from a wall read raw 3), which makes the stream unusable for RGB-D consumers. Millimeters is also what this function already documents (// ...depth in mm, DepthMilli) and matches the standard ROS 16UC1 depth convention. If the concern is the 65.535 m range cap for long-range aerial use, I'm happy to make the scale configurable per capture (defaulting to mm), or to implement pixels-as-float for true float32 meters — let me know which you'd prefer.

[andrewjong]

Also:

And 16UC1 depth in millimeters is the established ROS/OpenNI/RealSense convention; relabeling ProjectAirSim's 16UC1 as integer meters would be a surprising contract for every downstream ROS consumer.

[LucasJSch]

Currently there's no float32 computation in the DepthMili variable. It's all with uint16.

Regarding the second comment: It's an interesting observation, but the most performant solution would be to compute that at a later stage.

[andrewjong]

Sorry for forwarding these responses through Claude! I'm still familiarizing with the code base, just trying to contribute what helped me resolve a bug during my usage. From what I understand though this seems to make sense. I think the main issue if we use meters here is the loss of precision for anything sub-meter? Please let me know if I'm misunderstanding!

Thanks for taking a look! Small clarification on the code: On "there's no float32 computation in DepthMilli, it's all uint16" — that's just not what the code says, in either version. The original line is:

float DepthMilli = SrcPixel.R.GetFloat();      // float32, from the fp16 texel
uint16 DepthUint16 = static_cast<uint16>(DepthMilli);

DepthMilli is declared float, and FFloat16::GetFloat() returns float32. The value lives as float32 right up to the final cast — in both the original and the patch. All the patch adds is one float32 multiply before that cast. So the premise that the multiply somehow happens "in uint16" (and would therefore add integer error) is a misreading; the multiply is exact to 1 ulp in float32, then a single quantization to uint16 happens either way. The only question is what one uint16 step means — 1 mm (patch) or 1 m (original).

For second comment:

Ah, I see — agreed that core_sim shouldn't adopt conventions because they're ROS conventions, and the ROS bridge is the right place for ROS-facing conversion. I only cited 16UC1-mm as corroboration; the actual reason for the ×1000 is internal to the sim: the wire is uint16, so a later stage can only convert what survives the cast, and integer meters destroy sub-meter precision at packing time (3.0 m and 3.9 m both arrive as 3 — nothing left for the bridge to compute). Any sub-meter scale works; mm matches this function's existing // ...depth in mm comment and DepthMilli naming. If the 65.535 m cap is a concern for long-range scenes, I'm happy to switch to centimeters (655 m range) or add a per-capture depth-scale setting instead — whichever you prefer.

[jonyMarino]

The depth milli is ultimately transmitted in 16 bits, so multiplying by 1000 in float16 would introduce a very small error; the biggest problem with float16 is overflow. There are different ways to tackle this, but for now, for ROS, what you can do is multiply the depth image you receive by 1000.

[jonyMarino]

The float32 should be converted to float16 or some other type that allows for a reduction in the amount of processing and copying in this step.

[andrewjong]

Ah okay! I see now, that makes sense. Implemented the change with some caveats:

from Claude:

Implemented in 37bdd2f: the depth wire now carries each pixel's render-target FFloat16 bit pattern verbatim (little-endian, new encoding label 16FC1, meters). The packing loop is now a pure bit copy — cheaper than both the old cast and the mm multiply — with no value conversion, no added quantization beyond the fp16 render target itself, no 65 m cap, and sky/no-hit arriving as +inf. Downstream decoders are updated in the same commit: the ROS2 C++ bridge decodes 16FC1 → standard ROS 32FC1 float meters (non-finite → NaN), the legacy Python rosbridge gets an equivalent converter, and the Python client's unpack_image returns a float16 array (the 16UC1 branches are kept for compatibility with older sims).

One small correction on the interim workaround suggested earlier ("for ROS, multiply the depth image you receive by 1000"): that couldn't work against the previous code — static_cast<uint16>(meters) is a value truncation, not a bit copy, so the wire carried whole meters and the sub-meter information was already destroyed before any later stage could scale it (a wall at 3.24 m arrived as exactly 3). With this commit the wire is the fp16 bits themselves, so downstream conversion is now lossless — verified end-to-end on our rig: the decoded 32FC1 stream shows fractional-millimeter depth structure and far geometry at ~4 km, both impossible under either previous packing.

Note this changes the depth wire encoding label from 16UC1 to 16FC1 intentionally, so older clients fail loudly (unknown encoding) rather than silently misreading fp16 bits as integers.

[jonyMarino]

Thanks for your contribution! Please review the comments.