:optimal

plugins/voxel_layer.cpp

@@ -602,18 +602,33 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
   local_origin.point.y = 0.0;
   local_origin.point.z = 0.0;
 
+  // Look up the sensor pose at the depth image's CAPTURE time, not the latest
+  // transform. The costmap update runs later than the frame was captured, so
+  // during rotation the latest pose orients the clearing frustum where the depth
+  // pixels were never measured from; the fan's free rays then sweep across and
+  // erase freshly marked cells, and the trailing side that gets erased flips
+  // with rotation direction. A stamped lookup keeps the frustum geometrically
+  // consistent with its own pixels. If the transform at that stamp is
+  // unavailable (stale / would extrapolate), skip clearing this cycle instead of
+  // clearing from a wrong pose. Falls back to latest only when the frame carries
+  // no stamp.
+  const robot::Time& depth_stamp = local_origin.header.stamp;
+  const tf3::Time query_time =
+      depth_stamp.isZero() ? tf3::Time() : tf3::Time(depth_stamp.sec, depth_stamp.nsec);
+
   robot_geometry_msgs::PointStamped global_origin;
   tf3::TransformStampedMsg tfm;
   try
   {
-    tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time());
+    tfm = tf_->lookupTransform(global_frame_, depth_frame, query_time);
     tf3::doTransform(local_origin, global_origin, tfm);
   }
   catch (tf3::TransformException& ex)
   {
     robot::log_error_throttle(
-        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
-        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what());
+        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s at t=%.3f: %s\n",
+        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(),
+        query_time.toSec(), ex.what());
     return false;
   }