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_;