Migrating RViz DisplayContext to tf2

[IanTheEngineer]

TLDR: is there any way of coaxing RViz DisplayContext to return a tf2_ros::Buffer pointer? If not, what will it take to upgrade RViz to tf2?

A little more background: I am in the midst of migrating MoveIt! to tf2, hopefully targeting the ROS Melodic release. This migration was requested by @dirk-thomas as the starting point for eventually porting MoveIt! to ROS2: moveit/moveit#745

One snag that I've run into is that RViz' DisplayContext FrameManager returns a tf::TransformListener, rather than a tf2_ros::Buffer, even on the ros2 branch:

/** @brief Return the FrameManager instance. */
virtual FrameManager* getFrameManager() const = 0;

/** @brief Convenience function: returns getFrameManager()->getTFClient(). */
virtual tf::TransformListener* getTFClient() const = 0;

MoveIt! uses this pointer to initialize its MoveGroupInterface for the motion_planning_rviz_plugin

move_group_.reset(new moveit::planning_interface::MoveGroupInterface(
opt, context_->getFrameManager()->getTFClientPtr(), ros::WallDuration(30, 0)));

It will need to receive a pointer to a tf2_ros::Buffer object. I have ported nearly all of MoveIt! to tf2, and this is (hopefully) one of the last roadblocks :)

[dhood]

While there's a ros2 branch on this repo, it's not up-to-date; the work moved to a fork, where tf2 is being used:
https://github.com/ros2/rviz/blob/2398b5055a225bcdf5bdf838124a6af4f5c93c97/rviz_common/include/rviz_common/frame_manager_iface.hpp#L240 (see ros2/rviz#52 for updates on buffer vs listener)

(@wjwwood I might remove that branch so it doesn't mislead people, yeah?)

@tfoote did you have any suggestions for a way @IanTheEngineer can access the underlying tf2 listener/buffer from the tf1 listener being provided, in the meantime?

[tfoote]

It's pretty well protected. You might be able to get the buffer through the protected member in a custom subclass.

[IanTheEngineer]

@tfoote it does seem to have a few defenses. Custom subclass it is...

Thanks for the info @dhood!

[wjwwood]

@dhood I removed the out-of-date ros2 branch on this repository. 👍

[IanTheEngineer]

Welp, with the help of subclassing, dynamic casting, and some questionable smart pointer management techniques, MoveIt! with TF2:

... which looks exactly the same, of course. Unfortunately, it core dumps after a few motion plans, but I'll chuck that up to those pesky pointers, and debug more later :)

I do think this ticket is still valid, as it would be nice not to have to hack around RViz' API, but it's less pressing with this workaround (if I can get rid of the seg faults). Thanks all for your help!

[IanTheEngineer]

Likely the workaround I described above will not work for MoveIt, as things get complex (and unsafe) when sharing a tf2_ros::Buffer that is not maintained by a shared_ptr. It might work in other, less multi-threaded scenarios, but I'm not confident. For posterity, here's the code:

#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>
class Transformer2 : public tf::Transformer
{
public:
    tf2_ros::Buffer& getBuffer()
    {
        return tf2_buffer_;
    }
};
...
tf2_ros::Buffer& tf_buffer = boost::dynamic_pointer_cast<Transformer2>(context_->getFrameManager()->getTFClientPtr())->getBuffer();

Note: the shared_ptr<tf::Transformer> from context_->getFrameManager()->getTFClientPtr() is not being saved off in this example, which means the Buffer could be deleted from under us. It's also not the most sterling example of object oriented encapsulation...

Instead, I will have MoveIt's planning_scene_monitor and move_group classes maintain their own buffer & transform listener instances, separate from RViz.

[wjwwood]

@tfoote is there a reason we do not want to provide a method to get the underlying tf2::Buffer from the tf::Transformer class in melodic?

This would allow us to provide both a tf and tf2 api in rviz without having two tf buffers. And we could avoid the subclassing trick that @IanTheEngineer said did not work.

@IanTheEngineer I don't suppose you have a backtrace of that crash and a branch where that version of the code was? I'd like to understand what's not working.

I'd really rather not have duplicated tf buffers.

[tfoote]

We could definitely add that to the api for Melodic if it would make supporting both easier.

[IanTheEngineer]

The "solution" that I presented above doesn't elegantly handle the reference counting of the already-allocated tf2_ros::Buffer object, and seems to slice the Transformer object before the Buffer is accessible. The stack trace for the latter issue:

#0  0x00007ffff6529428 in __GI_raise (sig=sig@entry=6)
at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff652b02a in __GI_abort () at abort.c:89
#2  0x00007ffff6521bd7 in __assert_fail_base (fmt=<optimized out>,
assertion=assertion@entry=0x7ffff78e89ee "px != 0",
file=file@entry=0x7ffff78e89b0 "/usr/include/boost/smart_ptr/shared_ptr.hpp", line=line@entry=648,
function=function@entry=0x7ffff78e8fa0 <boost::shared_ptr<Transformer2>::operator->() const::__PRETTY_FUNCTION__> "typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = Transformer2; typename boost::detail::sp_member_access<T>::type = Transformer2*]") at assert.c:92
#3  0x00007ffff6521c82 in __GI___assert_fail (
assertion=0x7ffff78e89ee "px != 0",
file=0x7ffff78e89b0 "/usr/include/boost/smart_ptr/shared_ptr.hpp", line=648,
function=0x7ffff78e8fa0 <boost::shared_ptr<Transformer2>::operator->() const::__PRETTY_FUNCTION__> "typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = Transformer2; typename boost::detail::sp_member_access<T>::type = Transformer2*]") at assert.c:101
#4  0x00007ffff77aee05 in boost::shared_ptr<Transformer2>::operator->
(this=0x7fffffffbc60)
at /usr/include/boost/smart_ptr/shared_ptr.hpp:648
#5  0x00007ffff77a974d in rviz::FrameManager::FrameManager (
this=0x1362990, tf=...)
at /data/users/imcmahon/dev/moveit_catkin/src/rviz/src/rviz/frame_manager.cpp:58
#6  0x00007ffff78bcac2 in rviz::VisualizationManager::VisualizationManager (this=0x1361f60, render_panel=0x7bda10, wm=0x8ebbd0, tf=...)
at /data/users/imcmahon/dev/moveit_catkin/src/rviz/src/rviz/visualization_manager.cpp:135
#7  0x00007ffff78abb62 in rviz::VisualizationFrame::initialize (
this=0x8ebba0, display_config_file=...)
at /data/users/imcmahon/dev/moveit_catkin/src/rviz/src/rviz/visualization_frame.cpp:329
#8  0x00007ffff78c5fdc in rviz::VisualizerApp::init (
this=0x7fffffffc7e0, argc=3, argv=0x7fffffffc928)
at /data/users/imcmahon/dev/moveit_catkin/src/rviz/src/rviz/visualizer_app.cpp:281
#9  0x00000000004011d2 in main (argc=4, argv=0x7fffffffc928)
at /data/users/imcmahon/dev/moveit_catkin/src/rviz/src/rviz/main.cpp:40

the diff:

diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp
index 6d9136d..998d219 100644
--- a/src/rviz/frame_manager.cpp
+++ b/src/rviz/frame_manager.cpp
@@ -32,10 +32,21 @@
 #include "properties/property.h"
 
 #include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 #include <ros/ros.h>
 
 #include <std_msgs/Float32.h>
 
+class Transformer2 : public tf::Transformer
+{
+public:
+    tf2_ros::Buffer& getBuffer()
+    {
+        return tf2_buffer_;
+    }
+};
+
 namespace rviz
 {
 
@@ -44,6 +55,8 @@ FrameManager::FrameManager(boost::shared_ptr<tf::TransformListener> tf)
   if (!tf) tf_.reset(new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), true));
   else tf_ = tf;
 
+  tf2_buffer_.reset(&boost::dynamic_pointer_cast<Transformer2>(tf_)->getBuffer());
+
diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h
index e75b5b6..61d7eee 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 @@ public:
   /** @brief Return a boost shared pointer to the tf::TransformListener used to receive transform data. */
   const boost::shared_ptr<tf::TransformListener>& getTFClientPtr() { return tf_; }
 
+  /** @brief Return a shared pointer to the tf2_ros::Buffer object used to receive tf2 transform data. */
+  const std::shared_ptr<tf2_ros::Buffer> 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 @@ private:
   M_Cache cache_;
 
   boost::shared_ptr<tf::TransformListener> tf_;
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;