fixed to handle TF timeout correctly

Author: mohammedari

opengl_ros/src/depth_image_projector_nodecore.cpp

@@ -81,6 +81,9 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
             tfListener_.lookupTransform(map_frame_id_, cameraInfoMsg->header.frame_id,
                 cameraInfoMsg->header.stamp, transform);
             getTransformMatrixArray(transform, depthToMap);
+        } else {
+            ROS_WARN_STREAM("TF wait transform timeout in depthCallback().");
+            return;
         }
 
         if (tfListener_.waitForTransform(map_frame_id_, fixed_frame_id_,
@@ -92,11 +95,20 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
                 map_frame_id_, cameraInfoMsg->header.stamp, 
                 fixed_frame_id_, tramsform);
             getTransformMatrixArray(tramsform, mapToPreviousMap);
+        } else {
+            ROS_WARN_STREAM("TF wait transform timeout in depthCallback().");
+
+            //stop accumulation in case required TF is already removed from the cache
+            previousTimestamp_ = cameraInfoMsg->header.stamp;    
+            return;
         }
     }
     catch (tf::TransformException& ex)
     {
         ROS_WARN("Transform Exception in depthCallback(). %s", ex.what());
+
+        //stop accumulation in case required TF is already removed from the cache
+        previousTimestamp_ = cameraInfoMsg->header.stamp;    
         return;
     }
 

opengl_ros/src/object_position_extractor_nodecore.cpp

@@ -229,6 +229,9 @@ void ObjectPositionExtractorNode::depthCallback(const sensor_msgs::Image::ConstP
         {
             tfListener_.lookupTransform(fixed_frame_id_, depth_frame_id_,
                 cv_ptr->header.stamp, transform_to_fixed_frame);
+        } else {
+            ROS_WARN_STREAM("TF wait transform timeout in depthCallback().");
+            return;
         }
     }
     catch (tf::TransformException& ex)
@@ -373,6 +376,9 @@ bool ObjectPositionExtractorNode::updateDepthToColor()
             getTransformMatrixArray(transform, latestDepthToColor_);
 
             depthToColorArrived_ = true;
+        } else {
+            ROS_WARN_STREAM("TF wait transform timeout in depthCallback().");
+            return false;
         }
     }
     catch (tf::TransformException& ex)