From 3f7575a921968035a9a11c2ce2287b50089d524f Mon Sep 17 00:00:00 2001
From: Tully Foote <tfoote@willowgarage.com>
Date: Thu, 11 Jul 2013 00:12:47 -0700
Subject: [PATCH] adding _chainAsVector method for
 https://github.com/ros/geometry/issues/18

---
 include/tf2/buffer_core.h |  5 ++++-
 src/buffer_core.cpp       | 29 +++++++++++++++++++++++++++++
 2 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/include/tf2/buffer_core.h b/include/tf2/buffer_core.h
index e9a02bb..ae2c332 100644
--- a/include/tf2/buffer_core.h
+++ b/include/tf2/buffer_core.h
@@ -294,7 +294,10 @@ class BufferCore
    */
   std::string _allFramesAsDot() const;
 
-
+  /** \brief Backwards compatabilityA way to see what frames are in a chain
+   * Useful for debugging
+   */
+  void _chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
 
 private:
 
diff --git a/src/buffer_core.cpp b/src/buffer_core.cpp
index 34fff9b..fb4a7c5 100644
--- a/src/buffer_core.cpp
+++ b/src/buffer_core.cpp
@@ -1388,4 +1388,33 @@ std::string BufferCore::_allFramesAsDot() const
 }
 
 
+void BufferCore::_chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame, std::vector<std::string>& output) const
+{
+  std::string error_string;
+
+  output.clear(); //empty vector
+
+  std::stringstream mstream;
+  boost::mutex::scoped_lock lock(frame_mutex_);
+
+  TransformStorage temp;
+
+  ///regular transforms
+  for (unsigned int counter = 1; counter < frames_.size(); counter ++)
+  {
+    TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter));
+    if (frame_ptr == NULL)
+      continue;
+    CompactFrameID frame_id_num;
+    if (frame_ptr->getData(ros::Time(), temp))
+        frame_id_num = temp.frame_id_;
+      else
+      {
+        frame_id_num = 0;
+      }
+      output.push_back(frameIDs_reverse[frame_id_num]);
+  }
+}
+
+
 } // namespace tf2