diff --git a/CMakeLists.txt b/CMakeLists.txt
index da590a1723..4349e40493 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -136,6 +136,7 @@ find_package(catkin REQUIRED
std_msgs
std_srvs
tf
+ tf2_ros
urdf
visualization_msgs
urdfdom_headers
diff --git a/package.xml b/package.xml
index 96da99b845..fd06947103 100644
--- a/package.xml
+++ b/package.xml
@@ -43,6 +43,7 @@
std_msgs
std_srvs
tf
+ tf2_ros
tinyxml
urdf
visualization_msgs
@@ -77,6 +78,7 @@
std_msgs
std_srvs
tf
+ tf2_ros
tinyxml
urdf
visualization_msgs
diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp
index 6d9136d06f..e1d64e8d54 100644
--- a/src/rviz/frame_manager.cpp
+++ b/src/rviz/frame_manager.cpp
@@ -32,6 +32,8 @@
#include "properties/property.h"
#include
+#include
+#include
#include
#include
@@ -44,6 +46,9 @@ FrameManager::FrameManager(boost::shared_ptr tf)
if (!tf) tf_.reset(new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), true));
else tf_ = tf;
+ tf2_buffer_ = std::make_shared(ros::Duration(10*60));
+ tf2_listener_.reset(new tf2_ros::TransformListener(*tf2_buffer_));
+
setSyncMode( SyncOff );
setPause(false);
}
diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h
index e75b5b61c2..61d7eee0f2 100644
--- a/src/rviz/frame_manager.h
+++ b/src/rviz/frame_manager.h
@@ -52,6 +52,12 @@ namespace tf
class TransformListener;
}
+namespace tf2_ros
+{
+class Buffer;
+class TransformListener;
+}
+
namespace rviz
{
class Display;
@@ -186,6 +192,9 @@ Q_OBJECT
/** @brief Return a boost shared pointer to the tf::TransformListener used to receive transform data. */
const boost::shared_ptr& getTFClientPtr() { return tf_; }
+ /** @brief Return a shared pointer to the tf2_ros::Buffer object used to receive tf2 transform data. */
+ const std::shared_ptr getTFBufferPtr() { return tf2_buffer_; };
+
/** @brief Create a description of a transform problem.
* @param frame_id The name of the frame with issues.
* @param stamp The time for which the problem was detected.
@@ -266,6 +275,8 @@ Q_OBJECT
M_Cache cache_;
boost::shared_ptr tf_;
+ std::shared_ptr tf2_buffer_;
+ std::unique_ptr tf2_listener_;
std::string fixed_frame_;
bool pause_;