Maintain & expose tf2 Buffer in shared_ptr for tf (#163)

- Adds a tf2_ros::Buffer via a public accessor method to expose to customers of Transformer - Maintains the tf2_ros::Buffer in a shared_ptr to safely share access to the Buffer object - As this is targeting Melodic, adds c++11 compile flags to grant access to std::shared_ptr's - Reorders the include_directories in the CMakeLists to ensure the headers exposed in this package are read *before* the system installed catkin_INCLUDE_DIRS (otherwise changes to tf source headers are never detected during a catkin_make on a system with ros-*-geometry installed)
ros · May 2, 2018 · 42995de · 42995de
1 parent 4ee2cff
commit 42995de
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 27 deletions.
diff --git a/tf/CMakeLists.txt b/tf/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8)
 project(tf)
 
+add_compile_options(-std=c++11)
+
 find_package(catkin REQUIRED COMPONENTS
     angles
     geometry_msgs
@@ -18,9 +20,9 @@ find_package(Boost REQUIRED COMPONENTS thread signals system)
 catkin_python_setup()
 
 include_directories(
+    include
     ${Boost_INCLUDE_DIR}
     ${catkin_INCLUDE_DIRS}
-    include
 )
 
 add_message_files(DIRECTORY msg FILES tfMessage.msg)

diff --git a/tf/include/tf/tf.h b/tf/include/tf/tf.h
@@ -38,6 +38,7 @@
 #include <vector>
 #include <sstream>
 #include <map>
+#include <memory>
 
 #include <tf/exceptions.h>
 #include "tf/time_cache.h"
@@ -315,7 +316,7 @@ class Transformer
   void setExtrapolationLimit(const ros::Duration& distance);
 
   /**@brief Get the duration over which this transformer will cache */
-  ros::Duration getCacheLength() { return tf2_buffer_.getCacheLength();}
+  ros::Duration getCacheLength() { return tf2_buffer_ptr_->getCacheLength();}
 
   /**
    * \brief Add a callback that happens when a new transform has arrived
@@ -333,9 +334,12 @@ class Transformer
   std::string getTFPrefix() const { return tf_prefix_;};
 
   //Declare that it is safe to call waitForTransform
-  void setUsingDedicatedThread(bool value) { tf2_buffer_.setUsingDedicatedThread(value);};
+  void setUsingDedicatedThread(bool value) { tf2_buffer_ptr_->setUsingDedicatedThread(value);};
   // Get the state of using_dedicated_thread_ from the buffer
-  bool isUsingDedicatedThread() { return tf2_buffer_.isUsingDedicatedThread();};
+  bool isUsingDedicatedThread() { return tf2_buffer_ptr_->isUsingDedicatedThread();};
+
+  // Get a copy of the shared_ptr containing the tf2_ros::Buffer object
+  std::shared_ptr<tf2_ros::Buffer> getTF2BufferPtr() { return tf2_buffer_ptr_;};
 
 protected:
 
@@ -384,7 +388,7 @@ class Transformer
 
 
 protected:
-  tf2_ros::Buffer tf2_buffer_;
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_ptr_;
 
 };
 

diff --git a/tf/src/tf.cpp b/tf/src/tf.cpp
@@ -209,7 +209,7 @@ std::string tf::strip_leading_slash(const std::string& frame_name)
 Transformer::Transformer(bool interpolating,
                                 ros::Duration cache_time):
   fall_back_to_wall_time_(false),
-  tf2_buffer_(cache_time)
+  tf2_buffer_ptr_(std::make_shared<tf2_ros::Buffer>(cache_time))
 {
 
 }
@@ -222,15 +222,15 @@ Transformer::~Transformer()
 
 void Transformer::clear()
 {
-  tf2_buffer_.clear();
+  tf2_buffer_ptr_->clear();
 }
 
 
 bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
 {
   geometry_msgs::TransformStamped msgtf;
   transformStampedTFToMsg(transform, msgtf);
-  return tf2_buffer_.setTransform(msgtf, authority);
+  return tf2_buffer_ptr_->setTransform(msgtf, authority);
 
 };
 
@@ -239,7 +239,7 @@ void Transformer::lookupTransform(const std::string& target_frame, const std::st
                      const ros::Time& time, StampedTransform& transform) const
 {
   geometry_msgs::TransformStamped output = 
-    tf2_buffer_.lookupTransform(strip_leading_slash(target_frame),
+    tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame),
                                 strip_leading_slash(source_frame), time);
   transformStampedMsgToTF(output, transform);
   return;
@@ -250,7 +250,7 @@ void Transformer::lookupTransform(const std::string& target_frame,const ros::Tim
                      const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
 {
   geometry_msgs::TransformStamped output = 
-    tf2_buffer_.lookupTransform(strip_leading_slash(target_frame), target_time,
+    tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame), target_time,
                                 strip_leading_slash(source_frame), source_time,
                                 strip_leading_slash(fixed_frame));
   transformStampedMsgToTF(output, transform);
@@ -350,15 +350,15 @@ bool Transformer::waitForTransform(const std::string& target_frame, const std::s
                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
                                    std::string* error_msg) const
 {
-  return tf2_buffer_.canTransform(strip_leading_slash(target_frame),
+  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame),
                                   strip_leading_slash(source_frame), time, timeout, error_msg);
 }
 
 
 bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
                            const ros::Time& time, std::string* error_msg) const
 {
-  return tf2_buffer_.canTransform(strip_leading_slash(target_frame),
+  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame),
                                   strip_leading_slash(source_frame), time, error_msg);
 }
 
@@ -367,7 +367,7 @@ bool Transformer::canTransform(const std::string& target_frame,const ros::Time&
                                const ros::Time& source_time, const std::string& fixed_frame,
                                std::string* error_msg) const
 {
-  return tf2_buffer_.canTransform(strip_leading_slash(target_frame), target_time,
+  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time,
                                   strip_leading_slash(source_frame), source_time,
                                   strip_leading_slash(fixed_frame), error_msg);
 };
@@ -377,21 +377,21 @@ bool Transformer::waitForTransform(const std::string& target_frame,const ros::Ti
                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
                                    std::string* error_msg) const
 {
-  return tf2_buffer_.canTransform(strip_leading_slash(target_frame), target_time,
+  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time,
                                   strip_leading_slash(source_frame), source_time,
                                   strip_leading_slash(fixed_frame), timeout, error_msg);
 };
 
 
 bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
 {
-  return tf2_buffer_._getParent(strip_leading_slash(frame_id), time, parent);
+  return tf2_buffer_ptr_->_getParent(strip_leading_slash(frame_id), time, parent);
 };
 
 
 bool Transformer::frameExists(const std::string& frame_id_str) const
 {
-  return tf2_buffer_._frameExists(strip_leading_slash(frame_id_str));
+  return tf2_buffer_ptr_->_frameExists(strip_leading_slash(frame_id_str));
 }
 
 void Transformer::setExtrapolationLimit(const ros::Duration& distance)
@@ -416,37 +416,37 @@ struct TimeAndFrameIDFrameComparator
 
 int Transformer::getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const
 {
-  CompactFrameID target_id = tf2_buffer_._lookupFrameNumber(strip_leading_slash(target_frame));
-  CompactFrameID source_id = tf2_buffer_._lookupFrameNumber(strip_leading_slash(source_frame));
+  CompactFrameID target_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(target_frame));
+  CompactFrameID source_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(source_frame));
 
-  return tf2_buffer_._getLatestCommonTime(source_id, target_id, time, error_string);
+  return tf2_buffer_ptr_->_getLatestCommonTime(source_id, target_id, time, error_string);
 }
 
 
 //@todo - Fix this to work with new data structures
 void Transformer::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
 {
-  tf2_buffer_._chainAsVector(target_frame, target_time, 
+  tf2_buffer_ptr_->_chainAsVector(target_frame, target_time,
                              source_frame, source_time, 
                              fixed_frame, output);
 }
 
 std::string Transformer::allFramesAsString() const
 {
-  return tf2_buffer_.allFramesAsString();
+  return tf2_buffer_ptr_->allFramesAsString();
 }
 
 std::string Transformer::allFramesAsDot(double current_time) const
 {
-  return tf2_buffer_._allFramesAsDot(current_time);
+  return tf2_buffer_ptr_->_allFramesAsDot(current_time);
 }
 
 
 bool Transformer::ok() const { return true; }
 
 void Transformer::getFrameStrings(std::vector<std::string> & vec) const
 {
-  tf2_buffer_._getFrameStrings(vec);
+  tf2_buffer_ptr_->_getFrameStrings(vec);
 }
 
 
@@ -572,10 +572,10 @@ void Transformer::transformPose(const std::string& target_frame, const ros::Time
 
 boost::signals2::connection Transformer::addTransformsChangedListener(boost::function<void(void)> callback)
 {
-  return tf2_buffer_._addTransformsChangedListener(callback);
+  return tf2_buffer_ptr_->_addTransformsChangedListener(callback);
 }
 
 void Transformer::removeTransformsChangedListener(boost::signals2::connection c)
 {
-  tf2_buffer_._removeTransformsChangedListener(c);
+  tf2_buffer_ptr_->_removeTransformsChangedListener(c);
 }
diff --git a/tf/src/transform_listener.cpp b/tf/src/transform_listener.cpp
@@ -41,13 +41,13 @@ std::string tf::remap(const std::string& frame_id)
 
 
 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
-  Transformer(true, max_cache_time), tf2_listener_(Transformer::tf2_buffer_, node_, spin_thread)
+  Transformer(true, max_cache_time), tf2_listener_(*Transformer::tf2_buffer_ptr_, node_, spin_thread)
 {
   //Everything is done inside tf2 init
 }
 
 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
-  Transformer(true, max_cache_time), node_(nh), tf2_listener_(Transformer::tf2_buffer_, nh, spin_thread)
+  Transformer(true, max_cache_time), node_(nh), tf2_listener_(*Transformer::tf2_buffer_ptr_, nh, spin_thread)
 {
   //Everything is done inside tf2 init
 }