(image_buffer) use forward list as buffer

Author: MatthijsBurgh

ed_sensor_integration/include/ed/kinect/image_buffer.h

@@ -5,9 +5,9 @@
 #include <rgbd/types.h>
 #include <geolib/datatypes.h>
 
+#include <forward_list>
 #include <memory>
 #include <mutex>
-#include <queue>
 #include <thread>
 
 namespace rgbd
@@ -69,17 +69,29 @@ class ImageBuffer
 
     std::unique_ptr<tf::TransformListener> tf_listener_;
 
-    std::queue<rgbd::ImageConstPtr> image_buffer_;
+    /**
+     * @brief Newer images should be pushed on the front. This will result in the front being the most recent and the back being the oldest
+     */
+    std::forward_list<rgbd::ImageConstPtr> image_buffer_;
 
     ros::CallbackQueue cb_queue_;
 
     std::pair<rgbd::ImageConstPtr, geo::Pose3D> recent_image_;
-    std::mutex recent_image_mutex_;
+    std::mutex recent_image_mutex_; // For protecting ImageBuffer::recent_image_
     std::unique_ptr<std::thread> worker_thread_ptr_;
-    bool shutdown_;
+    bool shutdown_; // Trigger to kill the worker thread
 
+    /**
+     * @brief Iterates over the buffer and tries to get TF for the most recent image. Deletes image and all older images when succesful
+     * or when image is too old from the buffer
+     * @return Indicates whether the most recent image has been updated
+     */
     bool getMostRecentImageTF();
 
+    /**
+     * @brief Calls ImageBuffer::getMostRecentImageTF on the specified frequency
+     * @param frequency Frequency for checking new images.
+     */
     void workerThreadFunc(const float frequency=20);
 
 };

ed_sensor_integration/src/kinect/image_buffer.cpp

@@ -127,74 +127,86 @@ bool ImageBuffer::getMostRecentImageTF()
     // - - - - - - - - - - - - - - - - - -
     // Fetch kinect image and place in image buffer
 
-    rgbd::ImageConstPtr rgbd_image = kinect_client_->nextImage();
-    if (!rgbd_image)
     {
-        ROS_DEBUG("[IMAGE_BUFFER] No new image from the RGBD client");
-        return false;
+        rgbd::ImageConstPtr new_image = kinect_client_->nextImage();
+        if (new_image)
+        {
+            image_buffer_.push_front(new_image);
+            ROS_DEBUG_STREAM_NAMED("image_buffer", "[IMAGE_BUFFER] New image from the RGBD client with timestamp: " << new_image->getTimestamp());
+        }
+        else
+        {
+            ROS_DEBUG_NAMED("image_buffer", "[IMAGE_BUFFER] No new image from the RGBD client");
+        }
     }
 
     geo::Pose3D sensor_pose;
 
-    try
-    {
-        tf::StampedTransform t_sensor_pose;
-        tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
-        geo::convert(t_sensor_pose, sensor_pose);
-    }
-    catch(tf::ExtrapolationException& ex)
+    for (std::forward_list<rgbd::ImageConstPtr>::iterator it = image_buffer_.begin(); it != image_buffer_.end(); ++it)
     {
+        rgbd::ImageConstPtr& rgbd_image = *it;
         try
         {
-            // Now we have to check if the error was an interpolation or extrapolation error (i.e., the image is too old or
-            // to new, respectively). If it is too old, discard it.
-
-            tf::StampedTransform latest_sensor_pose;
-            tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0), latest_sensor_pose);
-            // If image time stamp is older than latest transform, throw it out
-            if ( latest_sensor_pose.stamp_ > ros::Time(rgbd_image->getTimestamp()) )
-            {
-                ROS_ERROR_STREAM_DELAYED_THROTTLE(10, "[IMAGE_BUFFER] Image too old to look-up tf: image timestamp = " << std::fixed
-                                << ros::Time(rgbd_image->getTimestamp()));
-                ROS_WARN_STREAM("[IMAGE_BUFFER] Image too old to look-up tf: image timestamp = " << std::fixed
-                                << ros::Time(rgbd_image->getTimestamp()));
-                return false;
-            }
-            else
+            tf::StampedTransform t_sensor_pose;
+            tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
+            geo::convert(t_sensor_pose, sensor_pose);
+        }
+        catch (tf::ExtrapolationException& ex)
+        {
+            try
             {
-                if(!tf_listener_->waitForTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), ros::Duration(0.5)))
+                // Now we have to check if the error was an interpolation or extrapolation error (i.e., the image is too old or
+                // to new, respectively). If it is too old, discard it.
+                tf::StampedTransform latest_sensor_pose;
+                tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(0), latest_sensor_pose);
+                // If image time stamp is older than latest transform, the image is too old and the tf data is not available anymore
+                if ( latest_sensor_pose.stamp_ > ros::Time(rgbd_image->getTimestamp()) )
                 {
-                    ROS_ERROR_STREAM_DELAYED_THROTTLE(10, "[IMAGE_BUFFER] Image too new to look-up tf: image timestamp: " << ros::Time(rgbd_image->getTimestamp()) << ", what: " << ex.what());
-                    ROS_WARN_STREAM("[IMAGE_BUFFER] Image too new to look-up tf: image timestamp: " << ros::Time(rgbd_image->getTimestamp()) << ", what: " << ex.what());
+
+                    ROS_DEBUG_STREAM_NAMED("image_buffer", "[IMAGE_BUFFER] Image too old to look-up tf. Deleting all images older than timestamp: " << std::fixed
+                                           << ros::Time(rgbd_image->getTimestamp()));
+                    // Deleting this image and all older images
+                    image_buffer_.erase_after(it, image_buffer_.end());
                     return false;
                 }
-                tf::StampedTransform t_sensor_pose;
-                tf_listener_->lookupTransform(root_frame_, rgbd_image->getFrameId(), ros::Time(rgbd_image->getTimestamp()), t_sensor_pose);
-                geo::convert(t_sensor_pose, sensor_pose);
+                else
+                {
+                    // Image is too new; continue to next image, which is older
+                    ROS_DEBUG_STREAM_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Image too new to look-up tf: image timestamp: " << std::fixed << ros::Time(rgbd_image->getTimestamp()) << ", what: " << ex.what());
+                    continue;
+                }
+            }
+            catch (tf::TransformException& ex)
+            {
+                ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
+                ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
+                continue;
             }
         }
-        catch(tf::TransformException& exc)
+        catch (tf::TransformException& ex)
         {
-            ROS_ERROR_DELAYED_THROTTLE(10, "[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
-            ROS_WARN("[IMAGE_BUFFER] Could not get latest sensor pose (probably because tf is still initializing): %s", ex.what());
-            return false;
+            ROS_ERROR_DELAYED_THROTTLE_NAMED(10, "image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
+            ROS_WARN_NAMED("image_buffer", "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
+            continue;
         }
-    }
-    catch(tf::TransformException& ex)
-    {
-        ROS_ERROR_DELAYED_THROTTLE(10, "[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
-        ROS_WARN("[IMAGE_BUFFER] Could not get sensor pose: %s", ex.what());
-        return false;
-    }
 
-    // Convert from ROS coordinate frame to geolib coordinate frame
-    sensor_pose.R = sensor_pose.R * geo::Matrix3(1, 0, 0, 0, -1, 0, 0, 0, -1);
+        // Convert from ROS coordinate frame to geolib coordinate frame
+        sensor_pose.R = sensor_pose.R * geo::Matrix3(1, 0, 0, 0, -1, 0, 0, 0, -1);
 
-    std::lock_guard<std::mutex> lg(recent_image_mutex_);
-    recent_image_.first = rgbd_image;
-    recent_image_.second = sensor_pose;
+        {
+            std::lock_guard<std::mutex> lg(recent_image_mutex_);
+            recent_image_.first = rgbd_image;
+            recent_image_.second = sensor_pose;
+        }
 
-    return true;
+        // Deleting this image and all older images
+        image_buffer_.erase_after(it, image_buffer_.end());
+
+        return true;
+    }
+
+    // Not been able to update the most recent image/TF combo
+    return false;
 }
 
 // ----------------------------------------------------------------------------------------------------
@@ -204,7 +216,8 @@ void ImageBuffer::workerThreadFunc(const float frequency)
     ros::Rate r(frequency);
     while(!shutdown_)
     {
-        getMostRecentImageTF();
+        if (!getMostRecentImageTF())
+            ROS_ERROR_DELAYED_THROTTLE_NAMED(5, "image_buffer", "[IMAGE_BUFFER] Could not get a pose for any image in the buffer");
         r.sleep();
     }
 }