adding _chainAsVector method for ros/geometry#18

kheaactua · Jul 11, 2013 · 3f7575a · 3f7575a
1 parent c90a4f4
commit 3f7575a
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 1 deletion.
diff --git 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
@@ -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